Index: Makefile =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Makefile,v retrieving revision 1.27 retrieving revision 1.28 diff -u -d -r1.27 -r1.28 --- Makefile 8 Jan 2004 22:39:38 -0000 1.27 +++ Makefile 8 Sep 2004 19:21:28 -0000 1.28 @@ -7,6 +7,6 @@ @echo Nothing to be done here. dox docs doc: - docs/builddocs --update --tree --search --detailed + docs/builddocs --all --update --tree --search --detailed .PHONY: all update install dox docs doc \ No newline at end of file Index: README =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/README,v retrieving revision 1.6 retrieving revision 1.7 diff -u -d -r1.6 -r1.7 --- README 21 Jan 2004 05:37:34 -0000 1.6 +++ README 1 Apr 2004 00:16:51 -0000 1.7 @@ -1,7 +1,7 @@ Welcome to the Tekkotsu framework. Unless otherwise noted, all source code and related files are -Copyright 2003, Carnegie Mellon University AIBO Lab -Released under the GPL. +Copyright 2004, Carnegie Mellon University AIBO Lab +Released under the LGPL. Documentation and Usage Guides: ------------------------------ Index: entry.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/entry.h,v retrieving revision 1.1 retrieving revision 1.2 diff -u -d -r1.1 -r1.2 --- entry.h 25 Jul 2003 20:18:00 -0000 1.1 +++ entry.h 17 Jul 2004 03:38:01 -0000 1.2 @@ -2,3 +2,4 @@ // OPEN_R_SDK/OPEN_R/include/OPENR/core_macro.h:10 // which says #include "entry.h" (a stub-generated file, specific to each process) + Index: license.txt =================================================================== RCS file: license.txt diff -N license.txt --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ license.txt 1 Apr 2004 00:16:51 -0000 1.1 @@ -0,0 +1,504 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 2.1, February 1999 + + Copyright (C) 1991, 1999 Free Software Foundation, Inc. + 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + +[This is the first released version of the Lesser GPL. It also counts + as the successor of the GNU Library Public License, version 2, hence + the version number 2.1.] + + Preamble + + The licenses for most software are designed to take away your +freedom to share and change it. By contrast, the GNU General Public +Licenses are intended to guarantee your freedom to share and change +free software--to make sure the software is free for all its users. + + This license, the Lesser General Public License, applies to some +specially designated software packages--typically libraries--of the +Free Software Foundation and other authors who decide to use it. You +can use it too, but we suggest you first think carefully about whether +this license or the ordinary General Public License is the better +strategy to use in any particular case, based on the explanations below. + + When we speak of free software, we are referring to freedom of use, +not price. Our General Public Licenses are designed to make sure that +you have the freedom to distribute copies of free software (and charge +for this service if you wish); that you receive source code or can get +it if you want it; that you can change the software and use pieces of +it in new free programs; and that you are informed that you can do +these things. + + To protect your rights, we need to make restrictions that forbid +distributors to deny you these rights or to ask you to surrender these +rights. These restrictions translate to certain responsibilities for +you if you distribute copies of the library or if you modify it. + + For example, if you distribute copies of the library, whether gratis +or for a fee, you must give the recipients all the rights that we gave +you. You must make sure that they, too, receive or can get the source +code. If you link other code with the library, you must provide +complete object files to the recipients, so that they can relink them +with the library after making changes to the library and recompiling +it. And you must show them these terms so they know their rights. + + We protect your rights with a two-step method: (1) we copyright the +library, and (2) we offer you this license, which gives you legal +permission to copy, distribute and/or modify the library. + + To protect each distributor, we want to make it very clear that +there is no warranty for the free library. Also, if the library is +modified by someone else and passed on, the recipients should know +that what they have is not the original version, so that the original +author's reputation will not be affected by problems that might be +introduced by others. + + Finally, software patents pose a constant threat to the existence of +any free program. We wish to make sure that a company cannot +effectively restrict the users of a free program by obtaining a +restrictive license from a patent holder. Therefore, we insist that +any patent license obtained for a version of the library must be +consistent with the full freedom of use specified in this license. + + Most GNU software, including some libraries, is covered by the +ordinary GNU General Public License. This license, the GNU Lesser +General Public License, applies to certain designated libraries, and +is quite different from the ordinary General Public License. We use +this license for certain libraries in order to permit linking those +libraries into non-free programs. + + When a program is linked with a library, whether statically or using +a shared library, the combination of the two is legally speaking a +combined work, a derivative of the original library. The ordinary +General Public License therefore permits such linking only if the +entire combination fits its criteria of freedom. The Lesser General +Public License permits more lax criteria for linking other code with +the library. + + We call this license the "Lesser" General Public License because it +does Less to protect the user's freedom than the ordinary General +Public License. It also provides other free software developers Less +of an advantage over competing non-free programs. These disadvantages +are the reason we use the ordinary General Public License for many +libraries. However, the Lesser license provides advantages in certain +special circumstances. + + For example, on rare occasions, there may be a special need to +encourage the widest possible use of a certain library, so that it becomes +a de-facto standard. To achieve this, non-free programs must be +allowed to use the library. A more frequent case is that a free +library does the same job as widely used non-free libraries. In this +case, there is little to gain by limiting the free library to free +software only, so we use the Lesser General Public License. + + In other cases, permission to use a particular library in non-free +programs enables a greater number of people to use a large body of +free software. For example, permission to use the GNU C Library in +non-free programs enables many more people to use the whole GNU +operating system, as well as its variant, the GNU/Linux operating +system. + + Although the Lesser General Public License is Less protective of the +users' freedom, it does ensure that the user of a program that is +linked with the Library has the freedom and the wherewithal to run +that program using a modified version of the Library. + + The precise terms and conditions for copying, distribution and +modification follow. Pay close attention to the difference between a +"work based on the library" and a "work that uses the library". The +former contains code derived from the library, whereas the latter must +be combined with the library in order to run. + + GNU LESSER GENERAL PUBLIC LICENSE + TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION + + 0. This License Agreement applies to any software library or other +program which contains a notice placed by the copyright holder or +other authorized party saying it may be distributed under the terms of +this Lesser General Public License (also called "this License"). +Each licensee is addressed as "you". + + A "library" means a collection of software functions and/or data +prepared so as to be conveniently linked with application programs +(which use some of those functions and data) to form executables. + + The "Library", below, refers to any such software library or work +which has been distributed under these terms. A "work based on the +Library" means either the Library or any derivative work under +copyright law: that is to say, a work containing the Library or a +portion of it, either verbatim or with modifications and/or translated +straightforwardly into another language. (Hereinafter, translation is +included without limitation in the term "modification".) + + "Source code" for a work means the preferred form of the work for +making modifications to it. For a library, complete source code means +all the source code for all modules it contains, plus any associated +interface definition files, plus the scripts used to control compilation +and installation of the library. + + Activities other than copying, distribution and modification are not +covered by this License; they are outside its scope. The act of +running a program using the Library is not restricted, and output from +such a program is covered only if its contents constitute a work based +on the Library (independent of the use of the Library in a tool for +writing it). Whether that is true depends on what the Library does +and what the program that uses the Library does. + + 1. You may copy and distribute verbatim copies of the Library's +complete source code as you receive it, in any medium, provided that +you conspicuously and appropriately publish on each copy an +appropriate copyright notice and disclaimer of warranty; keep intact +all the notices that refer to this License and to the absence of any +warranty; and distribute a copy of this License along with the +Library. + + You may charge a fee for the physical act of transferring a copy, +and you may at your option offer warranty protection in exchange for a +fee. + + 2. You may modify your copy or copies of the Library or any portion +of it, thus forming a work based on the Library, and copy and +distribute such modifications or work under the terms of Section 1 +above, provided that you also meet all of these conditions: + + a) The modified work must itself be a software library. + + b) You must cause the files modified to carry prominent notices + stating that you changed the files and the date of any change. + + c) You must cause the whole of the work to be licensed at no + charge to all third parties under the terms of this License. + + d) If a facility in the modified Library refers to a function or a + table of data to be supplied by an application program that uses + the facility, other than as an argument passed when the facility + is invoked, then you must make a good faith effort to ensure that, + in the event an application does not supply such function or + table, the facility still operates, and performs whatever part of + its purpose remains meaningful. + + (For example, a function in a library to compute square roots has + a purpose that is entirely well-defined independent of the + application. Therefore, Subsection 2d requires that any + application-supplied function or table used by this function must + be optional: if the application does not supply it, the square + root function must still compute square roots.) + +These requirements apply to the modified work as a whole. If +identifiable sections of that work are not derived from the Library, +and can be reasonably considered independent and separate works in +themselves, then this License, and its terms, do not apply to those +sections when you distribute them as separate works. But when you +distribute the same sections as part of a whole which is a work based +on the Library, the distribution of the whole must be on the terms of +this License, whose permissions for other licensees extend to the +entire whole, and thus to each and every part regardless of who wrote +it. + +Thus, it is not the intent of this section to claim rights or contest +your rights to work written entirely by you; rather, the intent is to +exercise the right to control the distribution of derivative or +collective works based on the Library. + +In addition, mere aggregation of another work not based on the Library +with the Library (or with a work based on the Library) on a volume of +a storage or distribution medium does not bring the other work under +the scope of this License. + + 3. You may opt to apply the terms of the ordinary GNU General Public +License instead of this License to a given copy of the Library. To do +this, you must alter all the notices that refer to this License, so +that they refer to the ordinary GNU General Public License, version 2, +instead of to this License. (If a newer version than version 2 of the +ordinary GNU General Public License has appeared, then you can specify +that version instead if you wish.) Do not make any other change in +these notices. + + Once this change is made in a given copy, it is irreversible for +that copy, so the ordinary GNU General Public License applies to all +subsequent copies and derivative works made from that copy. + + This option is useful when you wish to copy part of the code of +the Library into a program that is not a library. + + 4. You may copy and distribute the Library (or a portion or +derivative of it, under Section 2) in object code or executable form +under the terms of Sections 1 and 2 above provided that you accompany +it with the complete corresponding machine-readable source code, which +must be distributed under the terms of Sections 1 and 2 above on a +medium customarily used for software interchange. + + If distribution of object code is made by offering access to copy +from a designated place, then offering equivalent access to copy the +source code from the same place satisfies the requirement to +distribute the source code, even though third parties are not +compelled to copy the source along with the object code. + + 5. A program that contains no derivative of any portion of the +Library, but is designed to work with the Library by being compiled or +linked with it, is called a "work that uses the Library". Such a +work, in isolation, is not a derivative work of the Library, and +therefore falls outside the scope of this License. + + However, linking a "work that uses the Library" with the Library +creates an executable that is a derivative of the Library (because it +contains portions of the Library), rather than a "work that uses the +library". The executable is therefore covered by this License. +Section 6 states terms for distribution of such executables. + + When a "work that uses the Library" uses material from a header file +that is part of the Library, the object code for the work may be a +derivative work of the Library even though the source code is not. +Whether this is true is especially significant if the work can be +linked without the Library, or if the work is itself a library. The +threshold for this to be true is not precisely defined by law. + + If such an object file uses only numerical parameters, data +structure layouts and accessors, and small macros and small inline +functions (ten lines or less in length), then the use of the object +file is unrestricted, regardless of whether it is legally a derivative +work. (Executables containing this object code plus portions of the +Library will still fall under Section 6.) + + Otherwise, if the work is a derivative of the Library, you may +distribute the object code for the work under the terms of Section 6. +Any executables containing that work also fall under Section 6, +whether or not they are linked directly with the Library itself. + + 6. As an exception to the Sections above, you may also combine or +link a "work that uses the Library" with the Library to produce a +work containing portions of the Library, and distribute that work +under terms of your choice, provided that the terms permit +modification of the work for the customer's own use and reverse +engineering for debugging such modifications. + + You must give prominent notice with each copy of the work that the +Library is used in it and that the Library and its use are covered by +this License. You must supply a copy of this License. If the work +during execution displays copyright notices, you must include the +copyright notice for the Library among them, as well as a reference +directing the user to the copy of this License. Also, you must do one +of these things: + + a) Accompany the work with the complete corresponding + machine-readable source code for the Library including whatever + changes were used in the work (which must be distributed under + Sections 1 and 2 above); and, if the work is an executable linked + with the Library, with the complete machine-readable "work that + uses the Library", as object code and/or source code, so that the + user can modify the Library and then relink to produce a modified + executable containing the modified Library. (It is understood + that the user who changes the contents of definitions files in the + Library will not necessarily be able to recompile the application + to use the modified definitions.) + + b) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (1) uses at run time a + copy of the library already present on the user's computer system, + rather than copying library functions into the executable, and (2) + will operate properly with a modified version of the library, if + the user installs one, as long as the modified version is + interface-compatible with the version that the work was made with. + + c) Accompany the work with a written offer, valid for at + least three years, to give the same user the materials + specified in Subsection 6a, above, for a charge no more + than the cost of performing this distribution. + + d) If distribution of the work is made by offering access to copy + from a designated place, offer equivalent access to copy the above + specified materials from the same place. + + e) Verify that the user has already received a copy of these + materials or that you have already sent this user a copy. + + For an executable, the required form of the "work that uses the +Library" must include any data and utility programs needed for +reproducing the executable from it. However, as a special exception, +the materials to be distributed need not include anything that is +normally distributed (in either source or binary form) with the major +components (compiler, kernel, and so on) of the operating system on +which the executable runs, unless that component itself accompanies +the executable. + + It may happen that this requirement contradicts the license +restrictions of other proprietary libraries that do not normally +accompany the operating system. Such a contradiction means you cannot +use both them and the Library together in an executable that you +distribute. + + 7. You may place library facilities that are a work based on the +Library side-by-side in a single library together with other library +facilities not covered by this License, and distribute such a combined +library, provided that the separate distribution of the work based on +the Library and of the other library facilities is otherwise +permitted, and provided that you do these two things: + + a) Accompany the combined library with a copy of the same work + based on the Library, uncombined with any other library + facilities. This must be distributed under the terms of the + Sections above. + + b) Give prominent notice with the combined library of the fact + that part of it is a work based on the Library, and explaining + where to find the accompanying uncombined form of the same work. + + 8. You may not copy, modify, sublicense, link with, or distribute +the Library except as expressly provided under this License. Any +attempt otherwise to copy, modify, sublicense, link with, or +distribute the Library is void, and will automatically terminate your +rights under this License. However, parties who have received copies, +or rights, from you under this License will not have their licenses +terminated so long as such parties remain in full compliance. + + 9. You are not required to accept this License, since you have not +signed it. However, nothing else grants you permission to modify or +distribute the Library or its derivative works. These actions are +prohibited by law if you do not accept this License. Therefore, by +modifying or distributing the Library (or any work based on the +Library), you indicate your acceptance of this License to do so, and +all its terms and conditions for copying, distributing or modifying +the Library or works based on it. + + 10. Each time you redistribute the Library (or any work based on the +Library), the recipient automatically receives a license from the +original licensor to copy, distribute, link with or modify the Library +subject to these terms and conditions. You may not impose any further +restrictions on the recipients' exercise of the rights granted herein. +You are not responsible for enforcing compliance by third parties with +this License. + + 11. If, as a consequence of a court judgment or allegation of patent +infringement or for any other reason (not limited to patent issues), +conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot +distribute so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you +may not distribute the Library at all. For example, if a patent +license would not permit royalty-free redistribution of the Library by +all those who receive copies directly or indirectly through you, then +the only way you could satisfy both it and this License would be to +refrain entirely from distribution of the Library. + +If any portion of this section is held invalid or unenforceable under any +particular circumstance, the balance of the section is intended to apply, +and the section as a whole is intended to apply in other circumstances. + +It is not the purpose of this section to induce you to infringe any +patents or other property right claims or to contest validity of any +such claims; this section has the sole purpose of protecting the +integrity of the free software distribution system which is +implemented by public license practices. Many people have made +generous contributions to the wide range of software distributed +through that system in reliance on consistent application of that +system; it is up to the author/donor to decide if he or she is willing +to distribute software through any other system and a licensee cannot +impose that choice. + +This section is intended to make thoroughly clear what is believed to +be a consequence of the rest of this License. + + 12. If the distribution and/or use of the Library is restricted in +certain countries either by patents or by copyrighted interfaces, the +original copyright holder who places the Library under this License may add +an explicit geographical distribution limitation excluding those countries, +so that distribution is permitted only in or among countries not thus +excluded. In such case, this License incorporates the limitation as if +written in the body of this License. + + 13. The Free Software Foundation may publish revised and/or new +versions of the Lesser General Public License from time to time. +Such new versions will be similar in spirit to the present version, +but may differ in detail to address new problems or concerns. + +Each version is given a distinguishing version number. If the Library +specifies a version number of this License which applies to it and +"any later version", you have the option of following the terms and +conditions either of that version or of any later version published by +the Free Software Foundation. If the Library does not specify a +license version number, you may choose any version ever published by +the Free Software Foundation. + + 14. If you wish to incorporate parts of the Library into other free +programs whose distribution conditions are incompatible with these, +write to the author to ask for permission. For software which is +copyrighted by the Free Software Foundation, write to the Free +Software Foundation; we sometimes make exceptions for this. Our +decision will be guided by the two goals of preserving the free status +of all derivatives of our free software and of promoting the sharing +and reuse of software generally. + + NO WARRANTY + + 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO +WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. +EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR +OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY +KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE +IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE +LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME +THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN +WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY +AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU +FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR +CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE +LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING +RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A +FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF +SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH +DAMAGES. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Libraries + + If you develop a new library, and you want it to be of the greatest +possible use to the public, we recommend making it free software that +everyone can redistribute and change. You can do so by permitting +redistribution under these terms (or, alternatively, under the terms of the +ordinary General Public License). + + To apply these terms, attach the following notices to the library. It is +safest to attach them to the start of each source file to most effectively +convey the exclusion of warranty; and each file should have at least the +"copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This library is free software; you can redistribute it and/or + modify it under the terms of the GNU Lesser General Public + License as published by the Free Software Foundation; either + version 2.1 of the License, or (at your option) any later version. + + This library is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + Lesser General Public License for more details. + + You should have received a copy of the GNU Lesser General Public + License along with this library; if not, write to the Free Software + Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA + +Also add information on how to contact you by electronic and paper mail. + +You should also get your employer (if you work as a programmer) or your +school, if any, to sign a "copyright disclaimer" for the library, if +necessary. Here is a sample; alter the names: + + Yoyodyne, Inc., hereby disclaims all copyright interest in the + library `Frob' (a library for tweaking knobs) written by James Random Hacker. + + , 1 April 1990 + Ty Coon, President of Vice + +That's all there is to it! + + Index: Behaviors/BehaviorBase.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/BehaviorBase.h,v retrieving revision 1.13 retrieving revision 1.14 diff -u -d -r1.13 -r1.14 --- Behaviors/BehaviorBase.h 9 Feb 2004 22:45:28 -0000 1.13 +++ Behaviors/BehaviorBase.h 24 Mar 2004 06:38:21 -0000 1.14 @@ -7,8 +7,20 @@ #include //! The basis from which all other Behaviors should inherit -/*! Makes use of ReferenceCounter so that behaviors can automatically delete themselves if wanted\n - * Make sure your own DoStart and DoStop call BehaviorBase::DoStart (or Stop) to allow this behavior... otherwise you'll get memory leaks */ +/*! 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 an empty behavior boilerplate file to help you get started quickly, try + * Tekkotsu/docs/behavior_header.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. + */ class BehaviorBase : public ReferenceCounter, public EventListener { public: //! constructor @@ -80,9 +92,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.13 $ + * $Revision: 1.14 $ * $State: Exp $ - * $Date: 2004/02/09 22:45:28 $ + * $Date: 2004/03/24 06:38:21 $ */ #endif Index: Behaviors/Controller.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controller.cc,v retrieving revision 1.36 retrieving revision 1.40 diff -u -d -r1.36 -r1.40 --- Behaviors/Controller.cc 1 Mar 2004 21:16:48 -0000 1.36 +++ Behaviors/Controller.cc 17 Oct 2004 01:16:10 -0000 1.40 @@ -43,7 +43,7 @@ SharedObject leds; leds->setWeights(~FaceLEDMask,0); leds->setWeights(FaceLEDMask,.75); - display=motman->addMotion(leds,isControlling?MotionManager::kEmergencyPriority:MotionManager::kIgnoredPriority); + display=motman->addPersistentMotion(leds,isControlling?MotionManager::kEmergencyPriority:MotionManager::kIgnoredPriority); reset(); } @@ -58,7 +58,7 @@ sndman->ReleaseFile(config->controller.prev_snd); sndman->ReleaseFile(config->controller.read_snd); sndman->ReleaseFile(config->controller.cancel_snd); - erouter->forgetListener(this); + erouter->removeListener(this); reset(); gui_comm->printf("goodbye\n"); wireless->setDaemon(gui_comm,false); @@ -453,7 +453,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.36 $ + * $Revision: 1.40 $ * $State: Exp $ - * $Date: 2004/03/01 21:16:48 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Controller.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controller.h,v retrieving revision 1.28 retrieving revision 1.29 diff -u -d -r1.28 -r1.29 --- Behaviors/Controller.h 9 Feb 2004 22:45:28 -0000 1.28 +++ Behaviors/Controller.h 24 Mar 2004 05:35:00 -0000 1.29 @@ -25,10 +25,11 @@ * - '!cancel' - calls ControlBase::doCancel() of the current control * - '!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' where count is the number of times '!hello' has been sent. Good for detecting first connection after boot vs. a reconnect. + * - '!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. * - '!hilight [n1 [n2 [...]]]' - hilights zero, one, or more items in the menu * - '!input text' - calls ControlBase::takeInput(text) on the currently hilighted control(s) - * - any text not beginning with '!' - sent to ControlBase::takeInput() of the current control + * - '!set section.key=value' - will be sent to Config::setValue(section,key,value) + * - any text not beginning with '!' - sent to ControlBase::takeInput() of the current control * * In return, to send the menus to the GUI, the following messages are sent: (newlines are required where shown) * - 'push' - signals a submenu has been activated @@ -211,9 +212,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.28 $ + * $Revision: 1.29 $ * $State: Exp $ - * $Date: 2004/02/09 22:45:28 $ + * $Date: 2004/03/24 05:35:00 $ */ #endif Index: Behaviors/StateNode.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/StateNode.cc,v retrieving revision 1.13 retrieving revision 1.17 diff -u -d -r1.13 -r1.17 --- Behaviors/StateNode.cc 11 Nov 2003 00:08:09 -0000 1.13 +++ Behaviors/StateNode.cc 4 Oct 2004 22:57:56 -0000 1.17 @@ -18,6 +18,14 @@ trans->addSource(this); } +StateNode* StateNode::addNode(StateNode* node) { + nodes.push_back(node); + node->AddReference(); + if ( node->parent == NULL ) + node->parent = this; + return node; +} + void StateNode::DoStart() { BehaviorBase::DoStart(); if(!issetup) { @@ -25,7 +33,7 @@ issetup=true; } for(std::vector::iterator it=transitions.begin(); it!=transitions.end(); it++) - (*it)->DoStart(); + if ( !(*it)->isActive() ) (*it)->DoStart(); erouter->postEvent(EventBase::stateMachineEGID,reinterpret_cast(this),EventBase::activateETID,0,getName(),1); if(parent!=NULL) parent->transitionTo(this); @@ -33,7 +41,6 @@ void StateNode::DoStop() { for(std::vector::iterator it=transitions.begin(); it!=transitions.end(); it++) { - ASSERT((*it)->isActive(),"Inactive transition"); if((*it)->isActive()) (*it)->DoStop(); } @@ -67,10 +74,10 @@ * @brief Describes StateNode, which is both a state machine controller as well as a node within a state machine itself * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.13 $ + * $Revision: 1.17 $ * $State: Exp $ - * $Date: 2003/11/11 00:08:09 $ + * $Date: 2004/10/04 22:57:56 $ */ Index: Behaviors/StateNode.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/StateNode.h,v retrieving revision 1.9 retrieving revision 1.10 diff -u -d -r1.9 -r1.10 --- Behaviors/StateNode.h 30 Oct 2003 23:23:15 -0000 1.9 +++ Behaviors/StateNode.h 4 Oct 2004 22:57:56 -0000 1.10 @@ -28,8 +28,8 @@ //!Returns the std::vector of transitions so you can modify them yourself if need be std::vector& getTransitions() { return transitions; } - //!Adds a StateNode to #nodes so it can be automatically dereferenced later, returns what it's passed (for convenience), calls AddReference() on @a node - virtual StateNode* addNode(StateNode* node) { nodes.push_back(node); node->AddReference(); return node; } + //!Adds a StateNode to #nodes so it can be automatically dereferenced later, returns what it's passed (for convenience), calls AddReference() on @a node. Also sets the node's parent to #this if it is null. + virtual StateNode* addNode(StateNode* node); //!Returns the std::vector of nodes so you can modify them yourself if need be std::vector& getNodes() { return nodes; } @@ -92,11 +92,11 @@ * @brief Describes StateNode, which is both a state machine controller as well as a node within a state machine itself * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.10 $ * $State: Exp $ - * $Date: 2003/10/30 23:23:15 $ + * $Date: 2004/10/04 22:57:56 $ */ #endif Index: Behaviors/Transition.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transition.cc,v retrieving revision 1.6 retrieving revision 1.8 diff -u -d -r1.6 -r1.8 --- Behaviors/Transition.cc 11 Nov 2003 00:08:10 -0000 1.6 +++ Behaviors/Transition.cc 8 Oct 2004 00:08:34 -0000 1.8 @@ -3,13 +3,13 @@ #include "Wireless/Wireless.h" #include "SoundPlay/SoundManager.h" -void Transition::activate() { - //serr->printf("%s activate() - enter %d\n",getName().c_str(),get_time()); +void Transition::fire() { + //serr->printf("%s fire() - enter %d\n",getName().c_str(),get_time()); AddReference(); //just in case a side effect of this transition is to dereference the transition, we don't want to be deleted while still transitioning if(sound.size()!=0) - sndman->PlayFile(sound.c_str()); + sndman->PlayFile(sound); for(unsigned int i=0; iisActive()) //It's usually a bad idea to call DoStop/DoStart when it's already stopped/started... @@ -18,7 +18,7 @@ if(!dsts[i]->isActive()) dsts[i]->DoStart(); - //serr->printf("%s activate() - leave %d\n",getName().c_str(),get_time()); + //serr->printf("%s fire() - leave %d\n",getName().c_str(),get_time()); RemoveReference(); } @@ -26,10 +26,10 @@ * @brief Implements Transition, represents a transition between StateNodes. * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.8 $ * $State: Exp $ - * $Date: 2003/11/11 00:08:10 $ + * $Date: 2004/10/08 00:08:34 $ */ Index: Behaviors/Transition.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transition.h,v retrieving revision 1.4 retrieving revision 1.5 diff -u -d -r1.4 -r1.5 --- Behaviors/Transition.h 11 Nov 2003 00:08:10 -0000 1.4 +++ Behaviors/Transition.h 4 Oct 2004 22:59:57 -0000 1.5 @@ -3,6 +3,7 @@ #define INCLUDED_Transition_h_ #include "BehaviorBase.h" +#include "Wireless/Socket.h" #include class StateNode; @@ -12,7 +13,7 @@ * conditions on the transitions. Transitions are "smart" - they are * behaviors in and of themselves and can listen for events and use * the standard DoStart/DoStop interface. Based on the events they - * receive, they can activate() themselves and cause a state + * receive, they can fire() themselves and cause a state * transition. * * DoStart() will be called when this transition is 'active' - it @@ -20,10 +21,10 @@ * DoStop() is called. * * If the conditions are satisified for a transition, you should call - * activate() to do the appropriate notifications. + * fire() to do the appropriate notifications. * * Also note that a transition can have multiple sources and - * destinations. See activate(). + * destinations. See fire(). * * When setting up, the flow of additions follows the flow of * control. In other words, you add a transition to a source, and @@ -51,7 +52,13 @@ virtual ~Transition() {} //!call this when the transition should be made, base class version simply calls StateNode::DoStop() on each active of #srcs and StateNode::DoStart() on each inactive of #dsts, but you can override. - virtual void activate(); + virtual void fire(); + + //!deprecated: use #fire() instead + virtual void activate() { + serr->printf("Transition::activate() is deprecated. Use Transition::fire() instead.\n"); + fire(); + } virtual std::vector& getSources() { return srcs; } //!< returns a user-modifiable reference to the current source list virtual const std::vector& getSources() const { return srcs; } //!< returns a const reference to the current source list @@ -78,11 +85,11 @@ * @brief Describes Transition, represents a transition between StateNodes. * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.4 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2003/11/11 00:08:10 $ + * $Date: 2004/10/04 22:59:57 $ */ #endif Index: Behaviors/Controls/BatteryCheckControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/BatteryCheckControl.h,v retrieving revision 1.8 retrieving revision 1.9 diff -u -d -r1.8 -r1.9 --- Behaviors/Controls/BatteryCheckControl.h 19 Jan 2004 20:34:51 -0000 1.8 +++ Behaviors/Controls/BatteryCheckControl.h 7 Oct 2004 19:07:04 -0000 1.9 @@ -41,7 +41,7 @@ } //! stops listening for power events and sets display to invalid virtual void pause() { - erouter->forgetListener(this); + erouter->removeListener(this); display_id=MotionManager::invalid_MC_ID; } //! calls report() @@ -63,7 +63,7 @@ } //! stops listening for power events and sets display to invalid virtual void deactivate() { - erouter->forgetListener(this); + erouter->removeListener(this); display_id=MotionManager::invalid_MC_ID; } //! calls refresh() to redisplay with new information if it's not a vibration event @@ -112,9 +112,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.8 $ + * $Revision: 1.9 $ * $State: Exp $ - * $Date: 2004/01/19 20:34:51 $ + * $Date: 2004/10/07 19:07:04 $ */ #endif Index: Behaviors/Controls/ControlBase.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/ControlBase.h,v retrieving revision 1.20 retrieving revision 1.21 diff -u -d -r1.20 -r1.21 --- Behaviors/Controls/ControlBase.h 18 Feb 2004 21:11:25 -0000 1.20 +++ Behaviors/Controls/ControlBase.h 7 Oct 2004 22:15:37 -0000 1.21 @@ -123,12 +123,12 @@ virtual ControlBase& setDescription(const std::string d) { description=d; return *this; } //!< sets the description of the control virtual std::string getDescription() const { return description; } //!< returns a short description of what the control does - const std::vector& getSlots() const { return options; } //!< returns the vector of sub-controls - std::string getSlotName(unsigned int i) const; //!< returns the string that will appear in slot @a i - unsigned int slotsSize() const { return options.size(); } //!< returns the number of options available - void setSlot(unsigned int i,ControlBase* o); //!< sets @a i'th element of #options to @a o - void pushSlot(ControlBase* o); //!< sets next unused element of #options to @a o - void clearSlots(); //!< deletes each slot item and clears the slots + virtual const std::vector& getSlots() const { return options; } //!< returns the vector of sub-controls + virtual std::string getSlotName(unsigned int i) const; //!< returns the string that will appear in slot @a i + virtual unsigned int slotsSize() const { return options.size(); } //!< returns the number of options available + virtual void setSlot(unsigned int i,ControlBase* o); //!< sets @a i'th element of #options to @a o + virtual void pushSlot(ControlBase* o); //!< sets next unused element of #options to @a o + virtual void clearSlots(); //!< deletes each slot item and clears the slots virtual const std::vector& getHilights() const { return hilights; } //!< returns a vector of the indicies of hilighted slots virtual void setHilights(const std::vector& hi); //!< sets the hilighted slots @@ -169,8 +169,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.20 $ + * $Revision: 1.21 $ * $State: Exp $ - * $Date: 2004/02/18 21:11:25 $ + * $Date: 2004/10/07 22:15:37 $ */ #endif Index: Behaviors/Controls/EventLogger.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/EventLogger.cc,v retrieving revision 1.11 retrieving revision 1.13 diff -u -d -r1.11 -r1.13 --- Behaviors/Controls/EventLogger.cc 9 Jan 2004 04:38:52 -0000 1.11 +++ Behaviors/Controls/EventLogger.cc 7 Oct 2004 22:15:21 -0000 1.13 @@ -63,36 +63,17 @@ //!sends all events received to stdout and/or logfile void EventLogger::processEvent(const EventBase& event) { - std::ostringstream logdata; - logdata << event.getName(); - switch(event.getTypeID()) { - case EventBase::activateETID: - logdata << "\tA"; break; - case EventBase::statusETID: - logdata << "\tS"; break; - case EventBase::deactivateETID: - logdata << "\tD"; break; - case EventBase::numETIDs: - logdata << "\tErr"; break; - } - if(verbosity>=1) - logdata << '\t' << event.getDuration() << '\t' << event.getTimeStamp(); - if(verbosity>=2) - logdata << '\t' << event.getMagnitude(); - if(verbosity>=3) { - if(const LocomotionEvent * loco=dynamic_cast(&event)) - logdata << '\t' << loco->x << '\t' << loco->y << '\t' << loco->a; - if(const TextMsgEvent * text=dynamic_cast(&event)) - logdata << '\t' << text->getToken() << '\t' << text->getText(); - if(const VisionObjectEvent * vis=dynamic_cast(&event)) - logdata << '\t' << vis->getCenterX() << '\t' << vis->getCenterY() << '\t' << vis->getDistance(); - } - + std::string logdata = event.getDescription(true,verbosity); if(options[EventBase::numEGIDs+2]->getName()[1]=='X') - sout->printf("EVENT: %s\n",logdata.str().c_str()); + sout->printf("EVENT: %s\n",logdata.c_str()); checkLogFile(); if(logfile) - logfile << logdata.str() << endl; + logfile << logdata << endl; +} + +void EventLogger::clearSlots() { + erouter->removeListener(this); + ControlBase::clearSlots(); } void EventLogger::setStatus(unsigned int i, char c) { @@ -128,7 +109,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.11 $ + * $Revision: 1.13 $ * $State: Exp $ - * $Date: 2004/01/09 04:38:52 $ + * $Date: 2004/10/07 22:15:21 $ */ Index: Behaviors/Controls/EventLogger.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/EventLogger.h,v retrieving revision 1.5 retrieving revision 1.6 diff -u -d -r1.5 -r1.6 --- Behaviors/Controls/EventLogger.h 25 Sep 2003 15:26:10 -0000 1.5 +++ Behaviors/Controls/EventLogger.h 7 Oct 2004 22:15:20 -0000 1.6 @@ -11,6 +11,7 @@ public: //!constructor EventLogger(); + virtual ~EventLogger() { clearSlots(); } //!opens a custom (embedded) menu to toggle individual EGIDs virtual ControlBase* doSelect(); @@ -21,6 +22,8 @@ virtual void processEvent(const EventBase& event); protected: + virtual void clearSlots(); + //!sets the status char of slot @a i to @a c void setStatus(unsigned int i, char c); @@ -43,9 +46,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.5 $ - * $State: Rel $ - * $Date: 2003/09/25 15:26:10 $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2004/10/07 22:15:20 $ */ #endif Index: Behaviors/Controls/FileInputControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/FileInputControl.h,v retrieving revision 1.1 retrieving revision 1.2 diff -u -d -r1.1 -r1.2 --- Behaviors/Controls/FileInputControl.h 8 Jan 2004 22:39:54 -0000 1.1 +++ Behaviors/Controls/FileInputControl.h 23 Mar 2004 00:55:01 -0000 1.2 @@ -19,10 +19,13 @@ {} //! returns the path to file last selected - const std::string& getLastInput() { return myfile; } + virtual const std::string& getLastInput() { return myfile; } + + //! clears the last input (i.e. so you can easily tell later if new input is entered) + virtual void clearLastInput() { selectedFile(""); } //! pass pointer to an external string you wish to have set when a file is selected; NULL otherwise - void setStore(std::string* store) { file=store; } + virtual void setStore(std::string* store) { file=store; } protected: virtual ControlBase* selectedFile(const std::string& f) { @@ -46,9 +49,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.1 $ + * $Revision: 1.2 $ * $State: Exp $ - * $Date: 2004/01/08 22:39:54 $ + * $Date: 2004/03/23 00:55:01 $ */ #endif Index: Behaviors/Controls/FreeMemReportControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/FreeMemReportControl.h,v retrieving revision 1.6 retrieving revision 1.7 diff -u -d -r1.6 -r1.7 --- Behaviors/Controls/FreeMemReportControl.h 26 Sep 2003 03:28:38 -0000 1.6 +++ Behaviors/Controls/FreeMemReportControl.h 7 Oct 2004 19:07:04 -0000 1.7 @@ -26,7 +26,7 @@ virtual void DoStop() { isWarning=false; - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); } @@ -67,9 +67,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ - * $State: Rel $ - * $Date: 2003/09/26 03:28:38 $ + * $Revision: 1.7 $ + * $State: Exp $ + * $Date: 2004/10/07 19:07:04 $ */ #endif Index: Behaviors/Controls/HelpControl.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/HelpControl.cc,v retrieving revision 1.6 retrieving revision 1.7 diff -u -d -r1.6 -r1.7 --- Behaviors/Controls/HelpControl.cc 21 Jan 2004 20:18:49 -0000 1.6 +++ Behaviors/Controls/HelpControl.cc 14 Jul 2004 00:50:26 -0000 1.7 @@ -1,5 +1,7 @@ #include "HelpControl.h" +//#define HelpControl_HTML_ + ControlBase * HelpControl::activate(MotionManager::MC_ID disp_id, Socket * gui) { char * fmt=" * "; if(config->main.use_VT100) { @@ -33,7 +35,9 @@ unsigned int numlen=1; if(slots.size()>1) numlen=(int)(log(slots.size()-1)/log(10))+1; - //sout->printf("
    \n"); +#ifdef HelpControl_HTML_ + sout->printf("
      \n"); +#endif for(unsigned int i=0; i0 && !isspace(desc[len-1])) len--; +#ifdef HelpControl_HTML_ + fmt="%s
    1. %s: %s"; +#else if(config->main.use_VT100) fmt="\33[1m%s%*d. %s\33[0m: %s\n"; else fmt="%s%*d. %s: %s\n"; - //fmt="%s
    2. %s: %s"; +#endif sout->printf(fmt,prefix.c_str(),numlen,i,nm.c_str(),desc.substr(0,len).c_str()); while(lendesc.size()) len=desc.size(); } - //sout->printf("\n%s",desc.substr(0,len).c_str()); +#ifdef HelpControl_HTML_ + sout->printf("\n%s",desc.substr(0,len).c_str()); +#else sout->printf("%s%s\n",std::string(prefix.size(),' ').c_str(),desc.substr(0,len).c_str()); +#endif while(lenprintf("
      3. \n"); +#ifdef HelpControl_HTML_ + sout->printf("\n"); +#endif report(slots[i],pre,depth_remain-1); } - //sout->printf("
    \n"); +#ifdef HelpControl_HTML_ + sout->printf("
\n"); +#endif } /*! @file @@ -85,8 +99,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.7 $ * $State: Exp $ - * $Date: 2004/01/21 20:18:49 $ + * $Date: 2004/07/14 00:50:26 $ */ Index: Behaviors/Controls/LoadPostureControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/LoadPostureControl.h,v retrieving revision 1.10 retrieving revision 1.13 diff -u -d -r1.10 -r1.13 --- Behaviors/Controls/LoadPostureControl.h 26 Jul 2003 01:48:03 -0000 1.10 +++ Behaviors/Controls/LoadPostureControl.h 17 Oct 2004 01:16:10 -0000 1.13 @@ -36,7 +36,7 @@ } virtual void deactivate() { - erouter->forgetListener(this); + erouter->removeListener(this); } protected: @@ -46,7 +46,7 @@ SharedObject post; post->LoadFile(file.c_str()); MMAccessor(estopid)->setActive(false); - MotionManager::MC_ID id=motman->addMotion(post,MotionManager::kEmergencyPriority+1,true); + MotionManager::MC_ID id=motman->addPrunableMotion(post,MotionManager::kEmergencyPriority+1); erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); return this; } @@ -61,9 +61,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ - * $State: Rel $ - * $Date: 2003/07/26 01:48:03 $ + * $Revision: 1.13 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Controls/PostureEditor.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/PostureEditor.cc,v retrieving revision 1.3 retrieving revision 1.7 diff -u -d -r1.3 -r1.7 --- Behaviors/Controls/PostureEditor.cc 30 Jan 2004 01:48:02 -0000 1.3 +++ Behaviors/Controls/PostureEditor.cc 18 Oct 2004 19:53:02 -0000 1.7 @@ -1,11 +1,19 @@ #include "PostureEditor.h" #include "Motion/MMAccessor.h" +#include "Motion/EmergencyStopMC.h" +#include "Motion/MotionSequenceMC.h" +#include "Motion/LedMC.h" +#include "Events/EventRouter.h" #include "ValueEditControl.h" #include "NullControl.h" +#include "SoundPlay/SoundManager.h" -PostureEditor::PostureEditor() +typedef MotionSequenceMC reach_t; //!< just to save some typing + +PostureEditor::PostureEditor(MotionManager::MC_ID estop_ID) : ControlBase("Posture Editor","Allows you to load, save, and numerically edit the posture"), - poseMC(), poseID(MotionManager::invalid_MC_ID), lastSlot(NULL), loadPose(NULL), savePose(NULL) + pose(), reachID(MotionManager::invalid_MC_ID), + estopID(estop_ID), lastSlot(NULL), loadPose(NULL), savePose(NULL), pauseCalled(false) { // add load and save menus pushSlot(loadPose=new FileInputControl("Load Posture","Select a posture to open",config->motion.root)); @@ -15,51 +23,71 @@ // add submenu for weight editors ControlBase * weights; pushSlot(weights=new ControlBase("Weights","Set the weights for outputs")); - for(unsigned int i=0; ipushSlot(new ValueEditControl(tmp,&poseMC->getOutputCmd(i).weight)); - } + for(unsigned int i=0; ipushSlot(new ValueEditControl(outputNames[i],&pose(i).weight)); pushSlot(NULL); // a separator for clarity // add actual value editors - for(unsigned int i=0; i(tmp,&poseMC->getOutputCmd(i).value)); - } + for(unsigned int i=0; i(outputNames[i],&pose(i).value)); } ControlBase * PostureEditor::activate(MotionManager::MC_ID disp_id, Socket * gui) { + //cout << "activate" << endl; // start off with current pose - poseMC->takeSnapshot(); + pose.takeSnapshot(); // clear the LEDs though for(unsigned int i=LEDOffset; isetOutputCmd(i,0); - // add it to motion manager above estop - poseID=motman->addMotion(poseMC,MotionManager::kEmergencyPriority+1,false); + pose.setOutputCmd(i,0); + // add it the motion sequence we'll be using to move to changes + SharedObject reach; + reachID=motman->addPersistentMotion(reach); + // we'll need to know when estop is turned on or off + erouter->addListener(this,EventBase::estopEGID); // call super class return ControlBase::activate(disp_id,gui); } void PostureEditor::refresh() { + //cout << "refresh" << endl; + if(isEStopped()) { + erouter->addTimer(this,0,500); + options[0]=NULL; + } else { + options[0]=loadPose; + } if(lastSlot==loadPose) { // we just got back from the load menu - MMAccessor(poseID)->LoadFile(loadPose->getLastInput().c_str()); + pose.LoadFile(loadPose->getLastInput().c_str()); + updatePose(moveTime); } else if(lastSlot==savePose || savePose->getLastInput().size()>0) { // we just got back from the save menu - MMAccessor(poseID)->SaveFile(config->motion.makePath(savePose->getLastInput()).c_str()); + pose.SaveFile(config->motion.makePath(savePose->getLastInput()).c_str()); savePose->takeInput(""); + } else { + updatePose(moveTime/2); } lastSlot=NULL; + pauseCalled=false; ControlBase::refresh(); } void +PostureEditor::pause() { + //cout << "paused" << endl; + pauseCalled=true; + erouter->removeListener(this,EventBase::timerEGID); +} + +void PostureEditor::deactivate() { - motman->removeMotion(poseID); - poseID=MotionManager::invalid_MC_ID; + //cout << "deactivate" << endl; + motman->removeMotion(reachID); + reachID=MotionManager::invalid_MC_ID; + erouter->removeListener(this); ControlBase::deactivate(); } @@ -71,6 +99,50 @@ return ControlBase::doSelect(); } +void +PostureEditor::processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::estopEGID) { + if(e.getTypeID()==EventBase::deactivateETID) { + MMAccessor(reachID)->play(); + erouter->removeListener(this,EventBase::timerEGID); + if(!pauseCalled) + refresh(); + } else { + if(!pauseCalled) { + erouter->addTimer(this,0,500); + processEvent(EventBase(EventBase::timerEGID,0,EventBase::statusETID)); + } + } + } else if(e.getGeneratorID()==EventBase::timerEGID) { + //doing a manual copy instead of just takeSnapshot() so we don't disturb the LED settings + for(unsigned int i=PIDJointOffset; ioutputs[i]; + refresh(); + } else { + serr->printf("WARNING: PostureEditor unexpected event: %s\n",e.getName().c_str()); + } +} + +bool +PostureEditor::isEStopped() { + return MMAccessor(estopID)->getStopped(); +} + +void +PostureEditor::updatePose(unsigned int delay) { + bool paused=isEStopped(); + MMAccessor reach_acc(reachID); + reach_acc->clear(); + reach_acc->setPlayTime(delay); + reach_acc->setPose(pose); + reach_acc->setPlayTime(delay+100); + reach_acc->setPose(pose); + if(paused) + reach_acc->pause(); + else + reach_acc->play(); +} + /*! @file * @brief Describes PostureEditor, which allows numeric control of joints and LEDs @@ -78,7 +150,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.7 $ * $State: Exp $ - * $Date: 2004/01/30 01:48:02 $ + * $Date: 2004/10/18 19:53:02 $ */ Index: Behaviors/Controls/PostureEditor.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/PostureEditor.h,v retrieving revision 1.2 retrieving revision 1.4 diff -u -d -r1.2 -r1.4 --- Behaviors/Controls/PostureEditor.h 18 Jan 2004 10:16:56 -0000 1.2 +++ Behaviors/Controls/PostureEditor.h 18 Oct 2004 19:53:02 -0000 1.4 @@ -4,36 +4,43 @@ #include "ControlBase.h" #include "Shared/SharedObject.h" -#include "Motion/PostureMC.h" +#include "Motion/PostureEngine.h" #include "Motion/MotionManager.h" #include "StringInputControl.h" #include "FileInputControl.h" +#include "Events/EventListener.h" //! allows logging of events to the console or a file -class PostureEditor : public ControlBase { +class PostureEditor : public ControlBase, public EventListener { public: //! constructor - PostureEditor(); + explicit PostureEditor(MotionManager::MC_ID estop_ID); - //! adds #poseMC to MotionManager virtual ControlBase * activate(MotionManager::MC_ID disp_id, Socket * gui); - - //! if we're back from a child slot, it's either load or save, so we need to handle it - virtual void refresh(); - - //! removes #poseMC from MotionManager + virtual void refresh(); //!< if we're back from a child slot, it's either load or save, so we need to handle it + virtual void pause(); virtual void deactivate(); //! opens a custom (embedded) menu edit outputs virtual ControlBase* doSelect(); + //! listens for the EStop to be turned off before moving + virtual void processEvent(const EventBase& e); + protected: - SharedObject poseMC; //!< the actual memory space for the posture - MotionManager::MC_ID poseID; //!< current id for the posture + PostureEngine pose; //!< the current target posture + MotionManager::MC_ID reachID; //!< id of motion sequence used to slow "snapping" to positions + MotionManager::MC_ID estopID; //!< so we can check if the estop is active ControlBase* lastSlot; //!< the last accessed slot FileInputControl* loadPose; //!< the control for loading postures StringInputControl* savePose; //!< the control for saving postures + bool pauseCalled; //!< true if refresh hasn't been called since pause + + static const unsigned int moveTime=1500; //!< number of milliseconds to take to load a posture - individual joint changes will be done in half the time + + bool isEStopped(); //!< called to check status of estop + void updatePose(unsigned int delay); //!< called anytime pose is modified; uses #reachID to move to #pose if estop is off, setting up #ledID otherwise private: PostureEditor(const PostureEditor& ); //!< don't call @@ -46,9 +53,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.4 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/18 19:53:02 $ */ #endif Index: Behaviors/Controls/RunSequenceControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/RunSequenceControl.h,v retrieving revision 1.10 retrieving revision 1.14 diff -u -d -r1.10 -r1.14 --- Behaviors/Controls/RunSequenceControl.h 14 Jul 2003 06:55:14 -0000 1.10 +++ Behaviors/Controls/RunSequenceControl.h 18 Oct 2004 17:01:38 -0000 1.14 @@ -4,21 +4,31 @@ #include "FileBrowserControl.h" #include "Motion/MotionSequenceMC.h" +#include "Motion/EmergencyStopMC.h" +#include "Motion/LedMC.h" +#include "SoundPlay/SoundManager.h" +#include "Motion/MMAccessor.h" #include "Shared/TimeET.h" #include "Shared/Config.h" #include //! Upon activation, loads a position from a file name read from cin (stored in ms/data/motion...) -/*! The template parameter is passed to MotionSequenceMC's template parameter in order - * to specify the number of keyframes to reserve - larger values use more memory, but will allow - * you to load more complicated sequences. +/*! The template parameter is passed to MotionSequenceMC's template + * parameter in order to specify the number of keyframes to reserve - + * larger values use more memory, but will allow you to load more + * complicated sequences. + * + * The motion sequence doesn't actually start playing until the + * emergency stop is deactivated. This avoids either cutting off the + * beginning of the sequence while still in estop, or having to + * override the estop, which may be unexpected. */ template class RunSequenceControl : public FileBrowserControl, public EventListener { public: //! Constructor, sets filter to *.mot RunSequenceControl(const std::string& n, MotionManager::MC_ID estop_id) - : FileBrowserControl(n,"Runs a motion sequence from a user-specified file",config->motion.root), estopid(estop_id) + : FileBrowserControl(n,"Runs a motion sequence from a user-specified file",config->motion.root), estopid(estop_id), ledid(MotionManager::invalid_MC_ID), waitingFile() { setFilter("*.mot"); } @@ -26,24 +36,40 @@ //! this is to help reduce the twitch at the end (estop tries to go back to its position when this is removed) virtual void processEvent(const EventBase& event) { erouter->removeListener(this,event); - MMAccessor estop(estopid); - estop->takeSnapshot(); - estop->setActive(true); + runFile(); + motman->removeMotion(ledid); } protected: - //!does the actual loading of the MotionSequence - virtual ControlBase* selectedFile(const std::string& f) { + //! loads the motion sequence and runs it + void runFile() { //TimeET timer; - SharedObject< MotionSequenceMC > s(f.c_str()); + SharedObject< MotionSequenceMC > s(waitingFile.c_str()); //cout << "Load Time: " << timer.Age() << endl; - MMAccessor(estopid)->setActive(false); - MotionManager::MC_ID id=motman->addMotion(s,MotionManager::kEmergencyPriority+1,true); - erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); + motman->addPrunableMotion(s,MotionManager::kEmergencyPriority+1); + waitingFile=""; + } + + //!does the actual loading of the MotionSequence + virtual ControlBase* selectedFile(const std::string& f) { + waitingFile=f; + if(!MMAccessor(estopid)->getStopped()) { + runFile(); + } else { + //we have to wait for the estop to be turned off + sndman->PlayFile("donkey.wav"); + SharedObject led; + led->cycle(BotLLEDMask,1000,3,0,0); + led->cycle(BotRLEDMask,1000,3,0,500); + ledid=motman->addPersistentMotion(led); + erouter->addListener(this,EventBase::estopEGID,estopid,EventBase::deactivateETID); + } return this; } MotionManager::MC_ID estopid; //!< MC_ID of the emergency stop (so we can reset it to the end frame) + MotionManager::MC_ID ledid; //!< MC_ID of the led we use to signal there's a MotionSequence lined up + std::string waitingFile; //!< filename of the motion sequence waiting to load }; /*! @file @@ -52,9 +78,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ - * $State: Rel $ - * $Date: 2003/07/14 06:55:14 $ + * $Revision: 1.14 $ + * $State: Exp $ + * $Date: 2004/10/18 17:01:38 $ */ #endif Index: Behaviors/Controls/SaveWalkControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/SaveWalkControl.h,v retrieving revision 1.7 retrieving revision 1.8 diff -u -d -r1.7 -r1.8 --- Behaviors/Controls/SaveWalkControl.h 10 Jan 2004 20:10:27 -0000 1.7 +++ Behaviors/Controls/SaveWalkControl.h 26 Mar 2004 00:28:41 -0000 1.8 @@ -18,12 +18,8 @@ virtual ControlBase * takeInput(const std::string& msg) { if(msg.size()>0) { - std::string filename; - if(msg[0]=='/') - filename=msg; - else - filename=config->motion.root+"/"+msg; - MotionManager::MC_ID id = thewalk==NULL?walk_id:thewalk->getID(); + std::string filename=config->motion.makePath(msg); + MotionManager::MC_ID id = (thewalk==NULL ? walk_id : thewalk->getID() ); WalkMC* walk=thewalk; if(id!=MotionManager::invalid_MC_ID) walk = (WalkMC*)motman->checkoutMotion(id); @@ -52,8 +48,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.7 $ + * $Revision: 1.8 $ * $State: Exp $ - * $Date: 2004/01/10 20:10:27 $ + * $Date: 2004/03/26 00:28:41 $ */ #endif Index: Behaviors/Controls/SensorObserverControl.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/SensorObserverControl.cc,v retrieving revision 1.1 retrieving revision 1.2 diff -u -d -r1.1 -r1.2 --- Behaviors/Controls/SensorObserverControl.cc 23 Dec 2003 06:33:41 -0000 1.1 +++ Behaviors/Controls/SensorObserverControl.cc 7 Oct 2004 19:07:04 -0000 1.2 @@ -70,7 +70,7 @@ if(numListeners>0) erouter->addListener(this,EventBase::sensorEGID,0); else - erouter->forgetListener(this); + erouter->removeListener(this); } if(ans==this) refresh(); @@ -133,7 +133,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.1 $ + * $Revision: 1.2 $ * $State: Exp $ - * $Date: 2003/12/23 06:33:41 $ + * $Date: 2004/10/07 19:07:04 $ */ Index: Behaviors/Controls/StringInputControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/StringInputControl.h,v retrieving revision 1.4 retrieving revision 1.5 diff -u -d -r1.4 -r1.5 --- Behaviors/Controls/StringInputControl.h 25 Sep 2003 15:26:11 -0000 1.4 +++ Behaviors/Controls/StringInputControl.h 23 Mar 2004 00:55:01 -0000 1.5 @@ -26,7 +26,10 @@ } //! returns last call to takeInput() - virtual std::string getLastInput() { return lastInput; } + virtual const std::string& getLastInput() { return lastInput; } + + //! clears the last input (i.e. so you can easily tell later if new input is entered) + virtual void clearLastInput() { takeInput(""); } //! sets the prompt to give to the user virtual void setPrompt(const std::string& prompt) { userPrompt=prompt; } @@ -42,9 +45,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.4 $ - * $State: Rel $ - * $Date: 2003/09/25 15:26:11 $ + * $Revision: 1.5 $ + * $State: Exp $ + * $Date: 2004/03/23 00:55:01 $ */ #endif Index: Behaviors/Controls/ToggleControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/ToggleControl.h,v retrieving revision 1.3 retrieving revision 1.4 diff -u -d -r1.3 -r1.4 --- Behaviors/Controls/ToggleControl.h 18 Jan 2004 10:16:56 -0000 1.3 +++ Behaviors/Controls/ToggleControl.h 23 Mar 2004 00:55:01 -0000 1.4 @@ -6,7 +6,10 @@ #include "Shared/ReferenceCounter.h" //! a simple control for turning things on and off -/*! Can also be used for radio buttons - select one of a group */ +/*! Can also be used for radio buttons - select one of a group + * + * By using the #externalStore (setStore()/getStore()), you can use + * this instead of a ValueEditControl */ class ToggleControl : public NullControl { public: //! a little class for managing the currently active ToggleControl to allow radio buttons @@ -43,9 +46,9 @@ //!@name Constructors //! - ToggleControl() : NullControl("[ ] "), rg(NULL) {} - ToggleControl(const std::string& n, RadioGroup * rad=NULL) : NullControl("[ ] "+n), rg(NULL) { setRadioGroup(rad); } - ToggleControl(const std::string& n, const std::string& d, RadioGroup * rad=NULL) : NullControl("[ ] "+n,d), rg(NULL) { setRadioGroup(rad); } + ToggleControl() : NullControl("[ ] "), rg(NULL), externalStore(NULL) {} + ToggleControl(const std::string& n, RadioGroup * rad=NULL) : NullControl("[ ] "+n), rg(NULL), externalStore(NULL) { setRadioGroup(rad); } + ToggleControl(const std::string& n, const std::string& d, RadioGroup * rad=NULL) : NullControl("[ ] "+n,d), rg(NULL), externalStore(NULL) { setRadioGroup(rad); } //@} ~ToggleControl() { setRadioGroup(NULL); } //!< destructor @@ -92,6 +95,8 @@ } } name[1]=c; + if(externalStore!=NULL) + *externalStore=getStatus(); } //! returns true if there's a non-space as the status @@ -118,8 +123,13 @@ //! returns the current RadioGroup virtual RadioGroup * getRadioGroup() const { return rg; } + virtual void setStore(bool * s) { externalStore=s; *s=getStatus(); } //!< sets #externalStore (and updates its current setting) + virtual bool * getStore() const { return externalStore; } //!< returns #externalStore + protected: RadioGroup * rg; //!< pointer to an optional radio group to allow one-of-many selections + + bool * externalStore; //!< an external bit which should be kept in sync with current setting private: ToggleControl(const ToggleControl& ); //!< don't call @@ -132,9 +142,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.4 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/03/23 00:55:01 $ */ #endif Index: Behaviors/Controls/ValueEditControl.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/ValueEditControl.h,v retrieving revision 1.12 retrieving revision 1.13 diff -u -d -r1.12 -r1.13 --- Behaviors/Controls/ValueEditControl.h 19 Jan 2004 20:34:51 -0000 1.12 +++ Behaviors/Controls/ValueEditControl.h 7 Oct 2004 19:07:04 -0000 1.13 @@ -35,7 +35,7 @@ //!reads in current value from target virtual ControlBase * activate(MotionManager::MC_ID display, Socket * gui) { cur=*target; - erouter->forgetListener(this); + erouter->removeListener(this); return StringInputControl::activate(display,gui); } //! will increment/decrement the current and then assign it to the target when head buttons pressed @@ -167,9 +167,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.12 $ + * $Revision: 1.13 $ * $State: Exp $ - * $Date: 2004/01/19 20:34:51 $ + * $Date: 2004/10/07 19:07:04 $ */ #endif Index: Behaviors/Controls/WalkCalibration.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Controls/WalkCalibration.h,v retrieving revision 1.5 retrieving revision 1.6 diff -u -d -r1.5 -r1.6 --- Behaviors/Controls/WalkCalibration.h 18 Jan 2004 10:16:56 -0000 1.5 +++ Behaviors/Controls/WalkCalibration.h 7 Oct 2004 19:07:04 -0000 1.6 @@ -27,7 +27,7 @@ virtual void refresh(); virtual void deactivate() { - erouter->forgetListener(this); + erouter->removeListener(this); ControlBase::deactivate(); } @@ -125,9 +125,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.5 $ + * $Revision: 1.6 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/07 19:07:04 $ */ #endif Index: Behaviors/Controls/WaypointWalkControl.cc =================================================================== RCS file: Behaviors/Controls/WaypointWalkControl.cc diff -N Behaviors/Controls/WaypointWalkControl.cc --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Controls/WaypointWalkControl.cc 17 Oct 2004 01:16:10 -0000 1.11 @@ -0,0 +1,251 @@ +#include "WaypointWalkControl.h" +#include "ToggleControl.h" +#include "NullControl.h" +#include "FileInputControl.h" +#include "StringInputControl.h" +#include "ValueEditControl.h" +#include "Motion/MotionManager.h" +#include "Motion/WaypointWalkMC.h" +#include "Motion/WalkMC.h" +#include "Motion/MMAccessor.h" + +WaypointWalkControl::WaypointWalkControl() + : ControlBase("WaypointWalkControl","Allows interactive control and execution of a set of waypoints"), + isRunning(false), startstopCtl(NULL), loopCtl(NULL), addEgoWPCtl(NULL), + addOffWPCtl(NULL), addAbsWPCtl(NULL), loadCtl(NULL), saveCtl(NULL), + localizationCtl(NULL), listOffset(0), walk_id(MotionManager::invalid_MC_ID) +{ + pushSlot(startstopCtl=new NullControl("Execute","Begin running waypoint list")); + pushSlot(loopCtl=new ToggleControl("Loop Waypoints","When last waypoint is reached, start over")); + pushSlot(addEgoWPCtl=new NullControl("Add Egocentric Waypoint","Appends a new egocentric waypoint (heading and location relative) at the end of the list")); + pushSlot(addOffWPCtl=new NullControl("Add Offset Waypoint","Appends a new offset waypoint (location relative) at the end of the list")); + pushSlot(addAbsWPCtl=new NullControl("Add Absolute Waypoint","Appends a new absolute waypoint at the end of the list")); + pushSlot(loadCtl=new FileInputControl("Load Waypoints","Reads a path from a file",config->motion.root)); + loadCtl->setFilter("*.wyp"); + pushSlot(saveCtl=new StringInputControl("Save Waypoints","Writes the current path to a file")); + pushSlot(localizationCtl=new StringInputControl("Drift Error Correction","Enter 3 numbers 'x y a' reprenting current error")); + pushSlot(NULL); + listOffset=slotsSize(); +} + +ControlBase * +WaypointWalkControl::activate(MotionManager::MC_ID disp_id, Socket * gui) { + if(walk_id==MotionManager::invalid_MC_ID) { + SharedObject walk; + walk_id=motman->addPersistentMotion(walk); + } + return ControlBase::activate(disp_id,gui); +} + +void +WaypointWalkControl::refresh() { + if(saveCtl->getLastInput().size()>0) { + sout->printf("Attempting save to %s...\n",saveCtl->getLastInput().c_str()); + std::string path=config->motion.makePath(saveCtl->getLastInput()); + MMAccessor(walk_id)->SaveWaypointFile(path.c_str()); + saveCtl->clearLastInput(); + } + if(loadCtl->getLastInput().size()>0) { + sout->printf("Attempting load from %s...\n",loadCtl->getLastInput().c_str()); + MMAccessor(walk_id)->LoadWaypointFile(loadCtl->getLastInput().c_str()); + loadCtl->clearLastInput(); + } + if(localizationCtl->getLastInput().size()>0) { + float x=0,y=0,a=0; + sscanf(localizationCtl->getLastInput().c_str(),"%g %g %g",&x,&y,&a); + MMAccessor walk(walk_id); + walk->setCurPos(x+walk->getCurX(),y+walk->getCurY(),a+walk->getCurA()); + cout << "Position is now " << walk->getCurX() << ' ' << walk->getCurY() << ' ' << walk->getCurA() << endl; + localizationCtl->clearLastInput(); + } + + MMAccessor walk(walk_id); + + loopCtl->setStatus(walk->getIsLooping()); + + //rebuild waypoint list + + //clear old entries + for(unsigned int i=listOffset; igetWaypointList(); + unsigned int wpcnt=1; + for(WaypointWalkMC::WaypointListIter_t it=wplist.begin(); it!=wplist.end(); it=wplist.next(it)) { + char cname[50]; + sprintf(cname,"Waypoint %d",wpcnt++); + char desc[100]; + const char * pt=NULL; + switch(wplist[it].posType) { + case WaypointWalkMC::Waypoint::POSTYPE_EGOCENTRIC: + pt="^="; break; //uhh, the '^' is supposed to imply it's heading-relative :-} + case WaypointWalkMC::Waypoint::POSTYPE_OFFSET: + pt="+="; break; + case WaypointWalkMC::Waypoint::POSTYPE_ABSOLUTE: + pt="="; break; + } + sprintf(desc,"x%s%g, y%s%g, a%s%g, arc=%g", pt,wplist[it].x,pt,wplist[it].y, + (wplist[it].angleIsRelative?"+=":"="),wplist[it].angle,wplist[it].arc); + pushSlot(new WaypointEditControl(cname,desc,walk_id,it)); + } + ControlBase::refresh(); +} + +void +WaypointWalkControl::deactivate() { + motman->removeMotion(walk_id); + walk_id=MotionManager::invalid_MC_ID; + ControlBase::deactivate(); +} + +ControlBase* +WaypointWalkControl::doSelect() { + for(unsigned int i=0; isetName("Execute"); + startstopCtl->setDescription("Begin running waypoint list"); + MMAccessor(walk_id)->pause(); + } else { + isRunning=true; + startstopCtl->setName("Stop"); + startstopCtl->setDescription("Halt locomotion"); + MMAccessor(walk_id)->go(); + } + sndman->PlayFile(config->controller.select_snd); + return curctl; + } else if(curctl==loopCtl) { + MMAccessor(walk_id)->setIsLooping(!loopCtl->getStatus()); + 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); + return curctl; + } else if(curctl==addOffWPCtl) { + MMAccessor(walk_id)->addOffsetWaypoint(0,0,false,true,.1); + 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); + return curctl; + } + } + return ControlBase::doSelect(); +} + +WaypointWalkControl::WaypointEditControl::WaypointEditControl(const std::string& n, const std::string& d, MotionManager::MC_ID walkid, unsigned int waypointid) + : ControlBase(n,d), walk_id(walkid), waypoint_id(waypointid), up(NULL), down(NULL), del(NULL), set(NULL) +{ + pushSlot(up=new NullControl("Up (Move up)","Moves up in waypoint list")); + pushSlot(down=new NullControl("Down (Move down)","Moves down in waypoint list")); + pushSlot(del=new NullControl("Delete","Removes from waypoint list")); + pushSlot(set=new NullControl("Set as current goal","Starts trying to reach this location")); + pushSlot(NULL); + MMAccessor walk(walk_id); + WaypointWalkMC::Waypoint& curway=walk->getWaypointList()[waypoint_id]; + pushSlot(new ValueEditControl("X",&curway.x)); + pushSlot(new ValueEditControl("Y",&curway.y)); + pushSlot(new ValueEditControl("A",&curway.angle)); + pushSlot(new ValueEditControl("Arc",&curway.arc)); + pushSlot(new ValueEditControl("Speed (in m/s)",&curway.speed)); + pushSlot(new ValueEditControl("Turn Speed (in rad/s)",&curway.turnSpeed)); + char desc[256]; + snprintf(desc,256,"Types: EGO=%d, OFF=%d, ABS=%d",WaypointWalkMC::Waypoint::POSTYPE_EGOCENTRIC,WaypointWalkMC::Waypoint::POSTYPE_OFFSET,WaypointWalkMC::Waypoint::POSTYPE_ABSOLUTE); + pushSlot(new ValueEditControl("Pos. coord. system",desc,(int*)&curway.posType)); + ToggleControl * tmptgl; + pushSlot(tmptgl=new ToggleControl("Angle is relative")); + tmptgl->setStatus(curway.angleIsRelative); + tmptgl->setStore(&curway.angleIsRelative); + pushSlot(tmptgl=new ToggleControl("Track path")); + tmptgl->setStatus(curway.trackPath); + tmptgl->setStore(&curway.trackPath); +} + +ControlBase* WaypointWalkControl::WaypointEditControl::doSelect() { + for(unsigned int i=0; i(walk_id)->getWaypointList(); + list.swap(list.prev(waypoint_id),waypoint_id); + 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); + return NULL; + } else if(curctl==del) { + MMAccessor(walk_id)->getWaypointList().erase(waypoint_id); + sndman->PlayFile(config->controller.select_snd); + return NULL; + } else if(curctl==set) { + MMAccessor(walk_id)->setTargetWaypoint(waypoint_id); + sndman->PlayFile(config->controller.select_snd); + return NULL; + } + } + return ControlBase::doSelect(); +} + + + /* //basic + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + */ + + /* + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,1,-M_PI/2,true,.1); + walk->addEgocentricWaypoint(-1,0,0,false,.1); + walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1); + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + */ + + /* + walk->addEgocentricWaypoint(1,0,0,true,.1); + walk->addEgocentricWaypoint(0,1,0,true,.1); + walk->addEgocentricWaypoint(0,1,-M_PI/2,true,.1); + walk->addEgocentricWaypoint(-1,0,M_PI/2,false,.1); + walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1); + */ + + /* //circle1 + walk->addEgocentricArc(0,.5,0,true,.1,M_PI); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.1); + walk->addEgocentricArc(.5,0,M_PI/2,true,.1,M_PI); + walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.1); + */ + + /* //circle2 + walk->addEgocentricArc(0,.25,0,true,.1,M_PI); + walk->addEgocentricWaypoint(0,0,M_PI/2,true,.075); + walk->addEgocentricArc(.25,0,M_PI/2,true,.1,M_PI); + walk->addEgocentricWaypoint(0,0,-M_PI/2,true,.075); + */ + + + +/*! @file + * @brief Implements WaypointWalkControl, which allows interactive control and execution of a set of waypoints + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.11 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + Index: Behaviors/Controls/WaypointWalkControl.h =================================================================== RCS file: Behaviors/Controls/WaypointWalkControl.h diff -N Behaviors/Controls/WaypointWalkControl.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Controls/WaypointWalkControl.h 27 Jul 2004 14:33:28 -0000 1.4 @@ -0,0 +1,84 @@ +//-*-c++-*- +#ifndef INCLUDED_WaypointWalkControl_h_ +#define INCLUDED_WaypointWalkControl_h_ + +#include "ControlBase.h" +#include "Shared/SharedObject.h" + +class NullControl; +class ToggleControl; +class FileInputControl; +class StringInputControl; + +//! Allows interactive control and execution of a set of waypoints +class WaypointWalkControl : public ControlBase { +public: + //!constructor + WaypointWalkControl(); + + //! add #walk to ::motman + virtual ControlBase * activate(MotionManager::MC_ID disp_id, Socket * gui); + + //!will be called after each waypoint is reached so we can update the menu + virtual void refresh(); + + //! remove the #walk from ::motman + virtual void deactivate(); + + //!handles selection of menu items + virtual ControlBase* doSelect(); + + // sends all events received to stdout and/or logfile + //virtual void processEvent(const EventBase& event); + + //! handles editing of individual waypoints + class WaypointEditControl : public ControlBase { + public: + //!constructor + WaypointEditControl(const std::string& n, const std::string& d, MotionManager::MC_ID walkid, unsigned int waypointid); + //!handles selection of menu items + virtual ControlBase* doSelect(); + protected: + MotionManager::MC_ID walk_id; //!< id number of the WaypointWalk + unsigned int waypoint_id; //!< id of the waypoint this is editing + NullControl * up; //!< command to move up in list + NullControl * down;//!< command to move down in list + NullControl * del; //!< command to delete from list + NullControl * set; //!< command to start targeting this location + private: + WaypointEditControl(const WaypointEditControl&); //!< don't call + WaypointEditControl operator=(const WaypointEditControl&); //!< don't call + }; + +protected: + bool isRunning; //!< true if #walk is currently running + NullControl * startstopCtl; //!< start and stop waypoint running + ToggleControl * loopCtl; //!< repeat waypoints + NullControl * addEgoWPCtl; //!< start and stop waypoint running + NullControl * addOffWPCtl; //!< start and stop waypoint running + NullControl * addAbsWPCtl; //!< start and stop waypoint running + FileInputControl * loadCtl; //!< allows loading of a path + StringInputControl * saveCtl; //!< save a path to a file + StringInputControl * localizationCtl; //!< enter localization updates manually + + unsigned int listOffset; //!< the index of the first waypoint in the menu + + MotionManager::MC_ID walk_id; //!< id number of the walk we're using, so we can check it out before modifying it + +private: + WaypointWalkControl(const WaypointWalkControl&); //!< don't call + WaypointWalkControl operator=(const WaypointWalkControl&); //!< don't call +}; + +/*! @file + * @brief Describes WaypointWalkControl, which allows interactive control and execution of a set of waypoints + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.4 $ + * $State: Exp $ + * $Date: 2004/07/27 14:33:28 $ + */ + +#endif Index: Behaviors/Demos/ASCIIVisionBehavior.cc =================================================================== RCS file: Behaviors/Demos/ASCIIVisionBehavior.cc diff -N Behaviors/Demos/ASCIIVisionBehavior.cc --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/ASCIIVisionBehavior.cc 19 Oct 2004 16:37:00 -0000 1.1 @@ -0,0 +1,35 @@ +#include "ASCIIVisionBehavior.h" +#include "Events/FilterBankEvent.h" +#include "Vision/RawCameraGenerator.h" +#include "Wireless/Socket.h" + +const char ASCIIVisionBehavior::charMap[ASCIIVisionBehavior::charMapSize] = { + ' ','.','\'','`','^','"',',',':',';','~','-','!','i','l','I','>','<','+','?',')', + '1',']','|','/','t','f','j','r','n','u','v','c','z','x','Y','U','J','C','L','Q', + '0','O','Z','X','m','w','q','p','d','b','k','h','a','o','*','#','M','W','&','8', + '%','B','$','@' +}; + + +void +ASCIIVisionBehavior::processEvent(const EventBase& e) { + const FilterBankEvent& fbkevt=dynamic_cast(e); + + unsigned int layer = 1; + + char charimg[(fbkevt.getWidth(layer)+1)*fbkevt.getHeight(layer)+1]; + char * curchar=charimg; + unsigned char * image = fbkevt.getImage(layer, RawCameraGenerator::CHAN_Y); + for (unsigned int y = 0; y < fbkevt.getHeight(layer); y++) { + unsigned char * row = image + (y * fbkevt.getStride(layer)); + for (unsigned int x = 0; x < fbkevt.getWidth(layer); x++) { + unsigned char * pixel = row + (x * fbkevt.getIncrement(layer)); + *curchar++=charMap[(pixel[0]*charMapSize)/256]; + } + *curchar++='\n'; + } + *curchar='\0'; + + sout->printf("\n\n%s",charimg); +} + Index: Behaviors/Demos/ASCIIVisionBehavior.h =================================================================== RCS file: Behaviors/Demos/ASCIIVisionBehavior.h diff -N Behaviors/Demos/ASCIIVisionBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/ASCIIVisionBehavior.h 19 Oct 2004 00:46:54 -0000 1.2 @@ -0,0 +1,52 @@ +//-*-c++-*- + +// This is an empty Behavior template file. +// Replace ASCIIVisionBehavior and streams low-resolution ASCII-art of the camera image to sout as appropriate, and go to town! + +#ifndef INCLUDED_ASCIIVisionBehavior_h_ +#define INCLUDED_ASCIIVisionBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Events/EventRouter.h" + +//! streams low-resolution ASCII-art of the camera image to sout +class ASCIIVisionBehavior : public BehaviorBase { +public: + //! constructor + ASCIIVisionBehavior() : BehaviorBase() {} + + static const unsigned int charMapSize=64; + static const char charMap[charMapSize]; + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + erouter->addListener(this,EventBase::visRawCameraEGID); + } + + virtual void DoStop() { + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e); + + virtual std::string getName() const { return "ASCIIVisionBehavior"; } + + static std::string getClassDescription() { return "streams low-resolution ASCII-art of the camera image to sout"; } + +protected: + +}; + +/*! @file + * @brief Defines ASCIIVisionBehavior, which streams low-resolution ASCII-art of the camera image to sout + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.2 $ + * $State: Exp $ + * $Date: 2004/10/19 00:46:54 $ + */ + +#endif Index: Behaviors/Demos/AlanBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/AlanBehavior.h,v retrieving revision 1.9 retrieving revision 1.11 diff -u -d -r1.9 -r1.11 --- Behaviors/Demos/AlanBehavior.h 6 Feb 2004 04:06:50 -0000 1.9 +++ Behaviors/Demos/AlanBehavior.h 17 Oct 2004 01:16:10 -0000 1.11 @@ -27,7 +27,7 @@ //now do your code: // creates a PostureMC class to move the joint(s) and adds it to global MotionManager - pose_id=motman->addMotion(SharedObject(),false); + pose_id=motman->addPersistentMotion(SharedObject()); // subscribe to sensor updated events through the global EventRouter erouter->addListener(this,EventBase::sensorEGID,SensorSourceID::UpdatedSID); } @@ -35,7 +35,7 @@ virtual void DoStop() { //do your code first: motman->removeMotion(pose_id); // removes your posture controller - erouter->forgetListener(this); // stops getting events (and timers, if we had any) + erouter->removeListener(this); // stops getting events (and timers, if we had any) //but don't forget to call superclass at the end: BehaviorBase::DoStop(); @@ -70,7 +70,7 @@ //only really works on the ERS-210 or ERS-7 models - the others don't have a proper pressure sensor //(the 220's antenna-thing is close, but doesn't give a continuous range) std::cout << "Unknown model" << std::endl; - erouter->forgetListener(this); // stops getting events (and timers, if we had any) + erouter->removeListener(this); // stops getting events (and timers, if we had any) return; } @@ -117,9 +117,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.11 $ * $State: Exp $ - * $Date: 2004/02/06 04:06:50 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/AutoGetupBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/AutoGetupBehavior.h,v retrieving revision 1.9 retrieving revision 1.12 diff -u -d -r1.9 -r1.12 --- Behaviors/Demos/AutoGetupBehavior.h 25 Sep 2003 15:26:22 -0000 1.9 +++ Behaviors/Demos/AutoGetupBehavior.h 17 Oct 2004 01:16:10 -0000 1.12 @@ -26,7 +26,7 @@ } //! Stops listening for events virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); } //! Run appropriate motion script if the robot falls over @@ -53,7 +53,7 @@ else gu=config->motion.makePath("gu_front.mot"); SharedObject< MotionSequenceMC > getup(gu.c_str()); - MotionManager::MC_ID id=motman->addMotion(getup,MotionManager::kHighPriority); + MotionManager::MC_ID id=motman->addPrunableMotion(getup,MotionManager::kHighPriority); erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); waiting=true; } @@ -75,9 +75,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ - * $State: Rel $ - * $Date: 2003/09/25 15:26:22 $ + * $Revision: 1.12 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/BanditMachine.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/BanditMachine.h,v retrieving revision 1.13 retrieving revision 1.17 diff -u -d -r1.13 -r1.17 --- Behaviors/Demos/BanditMachine.h 8 Dec 2003 00:20:57 -0000 1.13 +++ Behaviors/Demos/BanditMachine.h 17 Oct 2004 01:16:10 -0000 1.17 @@ -65,7 +65,7 @@ lie->setOutputCmd(RFrLegOffset+RotatorOffset,.73); lie->setOutputCmd(LFrLegOffset+KneeOffset,.6); lie->setOutputCmd(RFrLegOffset+KneeOffset,.6); - liedown=motman->addMotion(lie); + liedown=motman->addPrunableMotion(lie); } virtual void DoStop() { @@ -93,7 +93,7 @@ press->setOutputCmd(idx,.3); press->setPlayTime(1500); press->setOutputCmd(idx,outputRanges[idx][MinRange]); - press_id=motman->addMotion(press,MotionManager::kStdPriority+1,false); + press_id=motman->addPersistentMotion(press,MotionManager::kStdPriority+1); } //!destructor virtual ~PressNode() { @@ -164,7 +164,7 @@ WaitNode(const char* n, StateNode* p, karmedbanditExp3_1& bandito) : StateNode(n,p), b(bandito), reward(false), leds_id(MotionManager::invalid_MC_ID) { - leds_id=motman->addMotion(SharedObject()); + leds_id=motman->addPersistentMotion(SharedObject()); } //! destructor virtual ~WaitNode() { @@ -178,7 +178,7 @@ leds->cflash(BotLLEDMask+BotRLEDMask,1,1000); } virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); b.reward(reward); cout << endl; reward=false; @@ -193,7 +193,7 @@ MMAccessor leds(leds_id); leds->cflash(MidLLEDMask+MidRLEDMask,1,100); } - erouter->forgetListener(this); + erouter->removeListener(this); } protected: karmedbanditExp3_1& b; //!< the class implimenting a k-armed bandit algorithm to pass the reward back to @@ -217,9 +217,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.13 $ + * $Revision: 1.17 $ * $State: Exp $ - * $Date: 2003/12/08 00:20:57 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/BatteryMonitorBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/BatteryMonitorBehavior.h,v retrieving revision 1.14 retrieving revision 1.19 diff -u -d -r1.14 -r1.19 --- Behaviors/Demos/BatteryMonitorBehavior.h 9 Jan 2004 04:41:05 -0000 1.14 +++ Behaviors/Demos/BatteryMonitorBehavior.h 17 Oct 2004 01:16:10 -0000 1.19 @@ -12,6 +12,7 @@ #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 @@ -43,7 +44,7 @@ virtual void DoStop() { if(pose!=NULL) stopWarning(); - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); } //! Adds a BatteryMonitorMC to motman if power goes low @@ -103,11 +104,11 @@ //! adds a pose and a timer to get the ears flipping void startWarning() { serr->printf("LOW BATTERY\n"); - pose_id=motman->addMotion(SharedObject(),MotionManager::kEmergencyPriority+1,false); + 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->addMotion(led,MotionManager::kEmergencyPriority+1); + led_id=motman->addPersistentMotion(led,MotionManager::kEmergencyPriority+1); setFlipper(true); erouter->addTimer(this,2,128,false); } @@ -137,6 +138,9 @@ pose->setOutputCmd(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 @@ -153,9 +157,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.14 $ + * $Revision: 1.19 $ * $State: Exp $ - * $Date: 2004/01/09 04:41:05 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/CameraBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/CameraBehavior.cc,v retrieving revision 1.4 retrieving revision 1.7 diff -u -d -r1.4 -r1.7 --- Behaviors/Demos/CameraBehavior.cc 5 Feb 2004 23:33:27 -0000 1.4 +++ Behaviors/Demos/CameraBehavior.cc 17 Oct 2004 01:16:10 -0000 1.7 @@ -33,12 +33,12 @@ } initIndex(); sndman->LoadFile("camera.wav"); - ledID=motman->addMotion(SharedObject()); + ledID=motman->addPersistentMotion(SharedObject()); erouter->addListener(this,camera_click); } void CameraBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); sndman->ReleaseFile("camera.wav"); motman->removeMotion(ledID); BehaviorBase::DoStop(); @@ -197,8 +197,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.4 $ + * $Revision: 1.7 $ * $State: Exp $ - * $Date: 2004/02/05 23:33:27 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/ChaseBallBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/ChaseBallBehavior.cc,v retrieving revision 1.8 retrieving revision 1.11 diff -u -d -r1.8 -r1.11 --- Behaviors/Demos/ChaseBallBehavior.cc 9 Dec 2003 17:33:14 -0000 1.8 +++ Behaviors/Demos/ChaseBallBehavior.cc 17 Oct 2004 01:16:10 -0000 1.11 @@ -12,13 +12,13 @@ void ChaseBallBehavior::DoStart() { BehaviorBase::DoStart(); - headpointer_id = motman->addMotion(SharedObject()); - walker_id = motman->addMotion(SharedObject()); + headpointer_id = motman->addPersistentMotion(SharedObject()); + walker_id = motman->addPersistentMotion(SharedObject()); erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); } void ChaseBallBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); motman->removeMotion(headpointer_id); motman->removeMotion(walker_id); BehaviorBase::DoStop(); @@ -65,9 +65,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.8 $ + * $Revision: 1.11 $ * $State: Exp $ - * $Date: 2003/12/09 17:33:14 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/CrashTestBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/CrashTestBehavior.h,v retrieving revision 1.1 retrieving revision 1.2 diff -u -d -r1.1 -r1.2 --- Behaviors/Demos/CrashTestBehavior.h 3 Oct 2003 03:40:17 -0000 1.1 +++ Behaviors/Demos/CrashTestBehavior.h 25 Aug 2004 01:07:45 -0000 1.2 @@ -30,7 +30,7 @@ static std::string getClassDescription() { // This string will be shown by the HelpControl or by the tooltips of the Controller GUI - return "A little demo of blocking output before a crash (yes, this crashes the AIBO)"; + return "A little demo of blocking output before a crash after output #33 (yes, this crashes the AIBO)"; } }; @@ -41,9 +41,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.1 $ - * $State: Rel $ - * $Date: 2003/10/03 03:40:17 $ + * $Revision: 1.2 $ + * $State: Exp $ + * $Date: 2004/08/25 01:07:45 $ */ #endif Index: Behaviors/Demos/DriveMeBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/DriveMeBehavior.cc,v retrieving revision 1.4 retrieving revision 1.7 diff -u -d -r1.4 -r1.7 --- Behaviors/Demos/DriveMeBehavior.cc 25 Jul 2003 20:18:04 -0000 1.4 +++ Behaviors/Demos/DriveMeBehavior.cc 17 Oct 2004 01:16:10 -0000 1.7 @@ -33,9 +33,9 @@ { BehaviorBase::DoStart(); // Insert walker into motion manager - walker_id = motman->addMotion(SharedObject()); + walker_id = motman->addPersistentMotion(SharedObject()); // Insert standing pose into motion manager - stand_id = motman->addMotion(stand, MotionManager::kStdPriority+1, false); + stand_id = motman->addPersistentMotion(stand, MotionManager::kStdPriority+1); // We listen to timers erouter->addListener(this, EventBase::timerEGID); // Turn on timer that goes off now to take us immediately to processEvent @@ -48,7 +48,7 @@ // We're not listening to timers anymore. erouter->removeTimer(this); // We're not listening to *anything* anymore! (also kills timers on its own) - erouter->forgetListener(this); + erouter->removeListener(this); // remove walker and stander from motion manager motman->removeMotion(walker_id); motman->removeMotion(stand_id); @@ -110,8 +110,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.4 $ - * $State: Rel $ - * $Date: 2003/07/25 20:18:04 $ + * $Revision: 1.7 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/ExploreMachine.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/ExploreMachine.h,v retrieving revision 1.16 retrieving revision 1.19 diff -u -d -r1.16 -r1.19 --- Behaviors/Demos/ExploreMachine.h 19 Jan 2004 20:34:56 -0000 1.16 +++ Behaviors/Demos/ExploreMachine.h 17 Oct 2004 01:16:10 -0000 1.19 @@ -43,7 +43,7 @@ } SharedObject walk; - walkid=motman->addMotion(walk); + walkid=motman->addPersistentMotion(walk); WalkNode * move=NULL; start=addNode(turn=new WalkNode(0,0,0.5f,this)); turn->setName(getName()+"::turn"); @@ -66,7 +66,7 @@ } virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); StateNode::DoStop(); } @@ -103,9 +103,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.16 $ + * $Revision: 1.19 $ * $State: Exp $ - * $Date: 2004/01/19 20:34:56 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/FollowHeadBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/FollowHeadBehavior.cc,v retrieving revision 1.5 retrieving revision 1.8 diff -u -d -r1.5 -r1.8 --- Behaviors/Demos/FollowHeadBehavior.cc 6 Jan 2004 01:00:44 -0000 1.5 +++ Behaviors/Demos/FollowHeadBehavior.cc 17 Oct 2004 01:16:10 -0000 1.8 @@ -23,7 +23,7 @@ void FollowHeadBehavior::DoStart() { BehaviorBase::DoStart(); //set up the shared motions - walker_id=motman->addMotion(SharedObject()); + walker_id=motman->addPersistentMotion(SharedObject()); //register for events and timers erouter->addListener(this,head_release); erouter->addListener(this,head_lock); @@ -34,7 +34,7 @@ void FollowHeadBehavior::DoStop() { //remove timers and listeners - erouter->forgetListener(this); + erouter->removeListener(this); //remove motion commands, set them to invalid motman->removeMotion(walker_id); walker_id=MotionManager::invalid_MC_ID; @@ -52,12 +52,12 @@ } else if(e==head_release) { cout << "release" << endl; - motman->addMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); + motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); erouter->addListener(this,clock); } else if(e==head_lock) { cout << "lock" << endl; - motman->addMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,1)); + motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,1)); 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; @@ -74,8 +74,8 @@ * * $Author: ejt $ * $Name $ - * $Revision: 1.5 $ + * $Revision: 1.8 $ * $State: Exp $ - * $Date: 2004/01/06 01:00:44 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/FreezeTestBehavior.h =================================================================== RCS file: Behaviors/Demos/FreezeTestBehavior.h diff -N Behaviors/Demos/FreezeTestBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/FreezeTestBehavior.h 27 Aug 2004 22:10:10 -0000 1.1 @@ -0,0 +1,56 @@ +//-*-c++-*- +#ifndef INCLUDED_FreezeTestBehavior_h_ +#define INCLUDED_FreezeTestBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Wireless/Wireless.h" + +//! Demonstrates an infinite loop condition in the Main process +class FreezeTestBehavior : public BehaviorBase { +public: + FreezeTestBehavior() : BehaviorBase() {} + + virtual void DoStart() { + //call superclass first for housekeeping: + BehaviorBase::DoStart(); + + //now do your code: + const unsigned int start=2500001; + serr->printf("Now computing all primes greater than %d... (this might take... forever)\n",start-1); + serr->printf("Motion process should be able to continue, but Main process will freeze.\n"); + for(unsigned int i=start;;i+=2) { + unsigned int j=3; + const unsigned int irt=(unsigned int)sqrt((double)i); + for(; j<=irt; j++) + if((i/j)*j==i) + break; + if(j==irt) + serr->printf("%d is prime\n",i); + } + //Hate to break it to you, but we're never going to get here... + } + + virtual std::string getName() const { + // Name is used for menus, or debugging. + return "FreezeTestBehavior"; + } + + static std::string getClassDescription() { + // This string will be shown by the HelpControl or by the tooltips of the Controller GUI + return "A little demo of a Main process infinite loop freeze (yes, this hangs the AIBO)"; + } + +}; + +/*! @file + * @brief Defines FreezeTestBehavior, demonstrates (lack of) blocking using serr to (not) pinpoint a crash + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2004/08/27 22:10:10 $ + */ + +#endif Index: Behaviors/Demos/GroundPlaneBehavior.h =================================================================== RCS file: Behaviors/Demos/GroundPlaneBehavior.h diff -N Behaviors/Demos/GroundPlaneBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/GroundPlaneBehavior.h 17 Oct 2004 01:16:10 -0000 1.6 @@ -0,0 +1,93 @@ +//-*-c++-*- + +#ifndef INCLUDED_GroundPlaneBehavior_h_ +#define INCLUDED_GroundPlaneBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/Kinematics.h" +#include "Motion/PIDMC.h" +#include "Motion/MotionManager.h" +#include "Shared/ERS7Info.h" +#include "Shared/ERS2xxInfo.h" +#include "Shared/WorldState.h" + +//! Reports the location of the center of the camera image on the ground plane +class GroundPlaneBehavior : public BehaviorBase { +public: + //! constructor + GroundPlaneBehavior() + : BehaviorBase(), + head_release(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::activateETID,0), + head_lock(EventBase::buttonEGID,(state->robotDesign&WorldState::ERS7Mask)?(int)ERS7Info::HeadButOffset:(int)ERS2xxInfo::HeadFrButOffset,EventBase::deactivateETID,0), + clock(EventBase::timerEGID,0,EventBase::statusETID,250) + { } + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + erouter->addListener(this,head_release); + erouter->addListener(this,head_lock); + processEvent(clock); + } + + virtual void DoStop() { + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e==clock) { + //This is the direction gravity is pulling - probably a good way to find out + //the attitude of the robot, assuming it is not moving. + //** Note that the LAccel sensor needs to be negated to match the coordinate system **// + NEWMAT::ColumnVector down=Kinematics::pack(state->sensors[BAccelOffset], + -state->sensors[LAccelOffset], + state->sensors[DAccelOffset]); + + //Just for kicks, lets report which leg is off the ground + cout << "I think leg " << kine->findUnusedLeg(down) << " is least used" << endl; + + //First we determine the ground plane + NEWMAT::ColumnVector p=kine->calculateGroundPlane(down); + cout << "Ground plane: " << p(1)<<"x + " << p(2)<<"y + " << p(3)<<"z = 1" << endl; + + //Project to ground plane - we do it twice here, once for camera frame and once for base frame + NEWMAT::ColumnVector ray(4); ray(1)=ray(2)=0; ray(3)=ray(4)=1; + NEWMAT::ColumnVector hit; + //cout <<"Current head:\n"<outputs[HeadOffset] <<' '<< state->outputs[HeadOffset+1] <<' '<< state->outputs[HeadOffset+2] << endl <getTransform(CameraFrameOffset); + hit=kine->projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,CameraFrameOffset); + cout << "Intersection_camera: (" << hit(1)<<','<projectToPlane(CameraFrameOffset,ray,BaseFrameOffset,p,BaseFrameOffset); + cout << "Intersection_base: (" << hit(1)<<','<addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); + erouter->addListener(this,clock); + processEvent(clock); + } else if(e==head_lock) { + motman->addPrunableMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,1)); + erouter->removeListener(this,clock); + } else { + ASSERT(false,"unprocessed event " << e.getName() << endl); + } + } + + virtual std::string getName() const { return "GroundPlaneBehavior"; } + + static std::string getClassDescription() { return "Reports the location of the center of the camera image on the ground plane"; } + +protected: + EventBase head_release, head_lock, clock; +}; + +/*! @file + * @brief Defines GroundPlaneBehavior, which reports the location of the center of the camera image on the ground plane + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Demos/HeadLevelBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/HeadLevelBehavior.h,v retrieving revision 1.6 retrieving revision 1.9 diff -u -d -r1.6 -r1.9 --- Behaviors/Demos/HeadLevelBehavior.h 19 Jan 2004 07:55:27 -0000 1.6 +++ Behaviors/Demos/HeadLevelBehavior.h 17 Oct 2004 01:16:10 -0000 1.9 @@ -5,14 +5,14 @@ #include "Behaviors/BehaviorBase.h" #include "Motion/MotionManager.h" #include "Motion/MMAccessor.h" -#include "Motion/HeadPointerMC.h" +#include "Motion/OldHeadPointerMC.h" #include "Motion/PIDMC.h" #include #include "Shared/ERS210Info.h" #include "Shared/ERS220Info.h" #include "Shared/ERS7Info.h" -//! Tests the head leveling code of HeadPointerMC +//! Tests the head leveling code of OldHeadPointerMC class HeadLevelBehavior : public BehaviorBase { public: //! constructor @@ -43,29 +43,29 @@ virtual void DoStart() { BehaviorBase::DoStart(); head->setJoints(state->outputs[HeadOffset+TiltOffset],state->outputs[HeadOffset+PanOffset],state->outputs[HeadOffset+RollOffset]); - head->setMode(HeadPointerMC::GravityRelative,true); + head->setMode(OldHeadPointerMC::GravityRelative,true); head->noMaxSpeed(); // this is probably pretty safe - the whole point is to keep the head still - head_id=motman->addMotion(head); + head_id=motman->addPersistentMotion(head); erouter->addListener(this,head_lock); erouter->addListener(this,head_release); } virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); motman->removeMotion(head_id); - head->setMode(HeadPointerMC::BodyRelative,false); + head->setMode(OldHeadPointerMC::BodyRelative,false); BehaviorBase::DoStop(); } virtual void processEvent(const EventBase &event) { if(event==head_lock) { for(unsigned int i=HeadOffset; isetJointValueFromMode((TPROffset_t)(i-HeadOffset),state->outputs[i],HeadPointerMC::BodyRelative); + head->setJointValueFromMode((TPROffset_t)(i-HeadOffset),state->outputs[i],OldHeadPointerMC::BodyRelative); motman->removeMotion(pid_id); pid_id=MotionManager::invalid_MC_ID; } else if(event==head_release) { - pid_id=motman->addMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0),MotionManager::kHighPriority,false); - //motman->addMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); + pid_id=motman->addPersistentMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0),MotionManager::kHighPriority); + //motman->addPersistentMotion(SharedObject(HeadOffset,HeadOffset+NumHeadJoints,0)); } else ASSERTRET(false,"received unasked for event "< head; //!< might as well just hang on to the whole memory region and reuse it, we can peek for most of our stuff + const SharedObject head; //!< might as well just hang on to the whole memory region and reuse it, we can peek for most of our stuff MotionManager::MC_ID head_id; //!< MCID of headpointer MotionManager::MC_ID pid_id; //!< MCID of pid controller }; @@ -86,9 +86,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.9 $ * $State: Exp $ - * $Date: 2004/01/19 07:55:27 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/KinematicSampleBehavior.h =================================================================== RCS file: Behaviors/Demos/KinematicSampleBehavior.h diff -N Behaviors/Demos/KinematicSampleBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/KinematicSampleBehavior.h 17 Oct 2004 01:16:10 -0000 1.6 @@ -0,0 +1,122 @@ +//-*-c++-*- +#ifndef INCLUDED_KinematicSampleBehavior_h_ +#define INCLUDED_KinematicSampleBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/PIDMC.h" +#include "Motion/PostureMC.h" +#include "Motion/MotionManager.h" +#include "Shared/SharedObject.h" +#include "Motion/roboop/robot.h" +#include "Shared/Config.h" +#include "Motion/Kinematics.h" + + +//! Uses kinematics to mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!) +class KinematicSampleBehavior : public BehaviorBase { +public: + //! constructor + KinematicSampleBehavior() + : BehaviorBase(), lastLeg(LFrLegOrder), poseID(MotionManager::invalid_MC_ID) + { } + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + poseID=motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::sensorEGID); + erouter->addListener(this,EventBase::buttonEGID); + } + + virtual void DoStop() { + motman->removeMotion(poseID); + poseID=MotionManager::invalid_MC_ID; + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::buttonEGID) { + switch(e.getSourceID()) { + case LFrPawOffset: + lastLeg=LFrLegOrder; break; + case RFrPawOffset: + lastLeg=RFrLegOrder; break; + case LBkPawOffset: + lastLeg=LBkLegOrder; break; + case RBkPawOffset: + lastLeg=RBkLegOrder; break; + default: + return; + } + if(e.getTypeID()==EventBase::activateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0); + motman->addPrunableMotion(relaxLeg); + MMAccessor pose_acc(poseID); + for(unsigned int i=0; isetOutputCmd(lastlegoff+i,OutputCmd::unused); + } else if(e.getTypeID()==EventBase::deactivateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1); + motman->addPrunableMotion(tightLeg); + } + + } else if(e.getGeneratorID()==EventBase::sensorEGID) { + //I'll use the pack and unpack functions here just to illustrate how to avoid + //needing to use NEWMAT data structures - but if you're going to be doing + //any significant math, you'll eventually want to get comfortable with NEWMAT... + + // (Actually, I think the NEWMAT version would be more readable...) + + /******** Determine location of target position ********/ + float link_x=60,link_y=0,link_z=0; // 6cm along x axis of selected joint + float obj_x=0, obj_y=0, obj_z=0; // these will hold the objective position + //this next line computes the link position, and stores result into obj_* + Kinematics::unpack(kine->linkToBase(getIndex(lastLeg))*Kinematics::pack(link_x,link_y,link_z), + obj_x,obj_y,obj_z); + + /******** Solve each leg for the point ********/ + MMAccessor pose_acc(poseID); // + for(unsigned int i=0; isolveLinkPosition(Kinematics::pack(m_x,m_y,obj_z), + getIndex((LegOrder_t)i), + Kinematics::pack(link_x,link_y,link_z)); + } + + //If you would like to verify the positiions of the back toes... (relative to body center) + //cout << "L: " << kine->getFrameInterestPoint(BaseFrameOffset,"ToeLBkPaw").t(); + //cout << "R: " << kine->getFrameInterestPoint(BaseFrameOffset,"ToeRBkPaw").t(); + + } else { + serr->printf("KinematicSampleBehavior: Unhandled event %s\n",e.getName().c_str()); + } + } + + virtual std::string getName() const { return "KinematicSampleBehavior"; } + + static std::string getClassDescription() { return "Uses kinematics to mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!)"; } + +protected: + unsigned int getIndex(LegOrder_t leg) { + //or try: return PawFrameOffset+leg; + return LegOffset+leg*JointsPerLeg+KneeOffset; + } + LegOrder_t lastLeg; + MotionManager::MC_ID poseID; +}; + +/*! @file + * @brief Defines KinematicSampleBehavior, which uses kinematics to mirror leg positions (note that it mirrors the paw position - not necessarily the joint angles used to get there!) + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Demos/KinematicSampleBehavior2.h =================================================================== RCS file: Behaviors/Demos/KinematicSampleBehavior2.h diff -N Behaviors/Demos/KinematicSampleBehavior2.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/KinematicSampleBehavior2.h 19 Oct 2004 17:06:51 -0000 1.9 @@ -0,0 +1,173 @@ +//-*-c++-*- +#ifndef INCLUDED_KinematicSampleBehavior2_h_ +#define INCLUDED_KinematicSampleBehavior2_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/PIDMC.h" +#include "Motion/PostureMC.h" +#include "Motion/MotionManager.h" +#include "Shared/SharedObject.h" +#include "Motion/roboop/robot.h" +#include "Shared/Config.h" +#include "Motion/Kinematics.h" + + +//! Uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh) +class KinematicSampleBehavior2 : public BehaviorBase { +public: + //! constructor + KinematicSampleBehavior2() + : BehaviorBase(), lastLeg(LFrLegOrder), poseID(MotionManager::invalid_MC_ID) + { } + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + poseID=motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::sensorEGID); + erouter->addListener(this,EventBase::buttonEGID); + } + + virtual void DoStop() { + motman->removeMotion(poseID); + poseID=MotionManager::invalid_MC_ID; + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::buttonEGID) { + switch(e.getSourceID()) { + case LFrPawOffset: + lastLeg=LFrLegOrder; break; + case RFrPawOffset: + lastLeg=RFrLegOrder; break; + case LBkPawOffset: + lastLeg=LBkLegOrder; break; + case RBkPawOffset: + lastLeg=RBkLegOrder; break; + default: + return; + } + if(e.getTypeID()==EventBase::activateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0); + motman->addPrunableMotion(relaxLeg); + MMAccessor pose_acc(poseID); + for(unsigned int i=0; isetOutputCmd(lastlegoff+i,OutputCmd::unused); + } else if(e.getTypeID()==EventBase::deactivateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1); + motman->addPrunableMotion(tightLeg); + } + + } else if(e.getGeneratorID()==EventBase::sensorEGID) { + //Plan A: + NEWMAT::ColumnVector obj(4); + switch(lastLeg) { + case LFrLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerBackLFrThigh,LowerOuterBackLFrThigh"); break; + case RFrLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerBackRFrThigh,LowerOuterBackRFrThigh"); break; + case LBkLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerFrontLBkThigh"); break; + case RBkLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerInnerFrontRBkThigh"); break; + } + if(obj(4)!=1) + return; + + unsigned int solveLink=PawFrameOffset+((lastLeg+2)%NumLegs); //swap front/back + NEWMAT::ColumnVector link(4); + switch(lastLeg) { + case LFrLegOrder: + link=kine->getLinkInterestPoint(solveLink,"ToeLBkPaw"); break; + case RFrLegOrder: + link=kine->getLinkInterestPoint(solveLink,"ToeRBkPaw"); break; + case LBkLegOrder: + link=kine->getLinkInterestPoint(solveLink,"LowerInnerBackLFrShin"); break; + case RBkLegOrder: + link=kine->getLinkInterestPoint(solveLink,"LowerInnerBackRFrShin"); break; + } + if(link(4)!=1) + return; + + //use the knee angle to assign distance from the solution point + float dist=state->outputs[LegOffset+lastLeg*JointsPerLeg+KneeOffset]; + dist*=30/outputRanges[LegOffset+lastLeg*JointsPerLeg+KneeOffset][MaxRange]; //scale to go up to 3 cm away + cout << "Distance is " << dist/10 << "cm" << endl; + float curlen=sqrt(link.SubMatrix(1,3,1,1).SumSquare()); + //Two ways to do the same thing: + if(lastLeg==LFrLegOrder || lastLeg==LBkLegOrder) + link.SubMatrix(1,3,1,1)*=(dist+curlen)/curlen; //scale the vector components individually + else + link(4)=curlen/(dist+curlen); //scale along the vector using the homogenous coordinate + + //Plan B: + /* + NEWMAT::ColumnVector obj(4); + switch(lastLeg) { + case LFrLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerLeftBackLFrShin,LowerRightBackLFrShin"); break; + case RFrLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"LowerLeftBackRFrShin,LowerRightBackRFrShin"); break; + case LBkLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"ToeLBkPaw"); break; + case RBkLegOrder: + obj=kine->getFrameInterestPoint(BaseFrameOffset,"ToeRBkPaw"); break; + } + if(obj(4)!=1) + return; + + unsigned int solveLink=PawFrameOffset+((lastLeg+2)%NumLegs); //swap front/back + NEWMAT::ColumnVector link(4); + switch(lastLeg) { + case LFrLegOrder: + link=kine->getLinkInterestPoint(solveLink,"ToeLBkPaw"); break; + case RFrLegOrder: + link=kine->getLinkInterestPoint(solveLink,"ToeRBkPaw"); break; + case LBkLegOrder: + link=kine->getLinkInterestPoint(solveLink,"LowerLeftBackLFrShin,LowerRightBackLFrShin"); break; + case RBkLegOrder: + link=kine->getLinkInterestPoint(solveLink,"LowerLeftBackRFrShin,LowerRightBackRFrShin"); break; + } + if(link(4)!=1) + return; + */ + + MMAccessor pose_acc(poseID); + pose_acc->solveLinkPosition(obj,solveLink,link); + + //If you would like to verify the positiions of the back toes... (relative to body center) + //cout << "L: " << kine->getFrameInterestPoint(BaseFrameOffset,"ToeLBkPaw").t(); + //cout << "R: " << kine->getFrameInterestPoint(BaseFrameOffset,"ToeRBkPaw").t(); + //cout << "Toe: " << pose_acc->getFrameInterestPoint(BaseFrameOffset,"ToeLBkPaw").t(); + //cout << "PawA: " << pose_acc->getFrameInterestPoint(BaseFrameOffset,"LBkPaw").t(); + //cout << "PawB: " << (pose_acc->frameToFrame(PawFrameOffset+LBkLegOrder,BaseFrameOffset)*Kinematics::pack(0,0,0)).t(); + + } else { + serr->printf("KinematicSampleBehavior2: Unhandled event %s\n",e.getName().c_str()); + } + } + + virtual std::string getName() const { return "KinematicSampleBehavior2"; } + + static std::string getClassDescription() { return "Uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh)"; } + +protected: + LegOrder_t lastLeg; + MotionManager::MC_ID poseID; +}; + +/*! @file + * @brief Defines KinematicSampleBehavior2, which uses kinematics to make the back toe (Toe{LR}BkPaw) touch the lower thigh (Lower{LeftBackL,RightBackR}FrThigh) + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.9 $ + * $State: Exp $ + * $Date: 2004/10/19 17:06:51 $ + */ + +#endif Index: Behaviors/Demos/LookForSoundBehavior.h =================================================================== RCS file: Behaviors/Demos/LookForSoundBehavior.h diff -N Behaviors/Demos/LookForSoundBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/LookForSoundBehavior.h 17 Oct 2004 01:16:10 -0000 1.4 @@ -0,0 +1,73 @@ +//-*-c++-*- +#ifndef INCLUDED_LookForSoundBehavior_h_ +#define INCLUDED_LookForSoundBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/HeadPointerMC.h" +#include "Events/DataEvent.h" + +//! Turns head to sound source, estimated by average volume difference between left and right ears +class LookForSoundBehavior : public BehaviorBase { +public: + //! constructor + LookForSoundBehavior() : BehaviorBase(), mc_id(MotionManager::invalid_MC_ID){} + + virtual void DoStart() + { + BehaviorBase::DoStart(); + + // You'll listen for sound and move your head + mc_id = motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::micOSndEGID); + } + + virtual void DoStop() + { + motman->removeMotion(mc_id); + erouter->removeListener(this); + BehaviorBase::DoStop(); + } + + virtual void processEvent(const EventBase& event) + { + if( event.getGeneratorID() == EventBase::micOSndEGID) { + // Get to the sound buffer, inevitable warning on line 37 + // getData() is not specified for const data + const DataEvent *de = + reinterpret_cast*>( &event); + + OSoundVectorData *svd = const_cast(de->getData()); + const short *d = ( const short *)svd->GetData(0); + + // Measure the energy of both channels + // Samples are interleaved [l,r] + double l = 0, r = 0; + int sz = svd->GetInfo(0)->frameSize; + for( int i = 0 ; i != sz ; i++){ + l += abs( d[2*i]); + r += abs( d[2*i+1]); + } + + // If there is sufficient energy coming in + if( l+r > sz*1000.){ + MMAccessor mc(mc_id); + double cur = state->outputs[ERS7Info::HeadOffset+1]; + if( r > 1.3*l) + // Move your head righward + mc->setJoints( 0, cur-.2*M_PI/(r/l), 0); + if( l > 1.3*r) + // Move your head leftward + mc->setJoints( 0, cur+.2*M_PI/(l/r), 0); + } + } + } + + virtual std::string getName() const { return "LookForSoundBehavior"; } + + static std::string getClassDescription() { return "Looking for Sound Behavior Class"; } + +protected: + MotionManager::MC_ID mc_id; +}; + +#endif Index: Behaviors/Demos/MCRepeater.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/MCRepeater.h,v retrieving revision 1.2 retrieving revision 1.4 diff -u -d -r1.2 -r1.4 --- Behaviors/Demos/MCRepeater.h 18 Jan 2004 10:16:56 -0000 1.2 +++ Behaviors/Demos/MCRepeater.h 17 Oct 2004 01:16:10 -0000 1.4 @@ -15,18 +15,18 @@ virtual void DoStart() { BehaviorBase::DoStart(); // do this first - MotionManager::MC_ID id=motman->addMotion(*mc,true); + MotionManager::MC_ID id=motman->addPrunableMotion(*mc); erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); } virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); // do this last } virtual void processEvent(const EventBase& /*e*/) { - erouter->forgetListener(this); - MotionManager::MC_ID id=motman->addMotion(*mc,true); + erouter->removeListener(this); + MotionManager::MC_ID id=motman->addPrunableMotion(*mc); erouter->addListener(this,EventBase::motmanEGID,id,EventBase::deactivateETID); } @@ -48,9 +48,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.4 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/MotionStressTestBehavior.h =================================================================== RCS file: Behaviors/Demos/MotionStressTestBehavior.h diff -N Behaviors/Demos/MotionStressTestBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/MotionStressTestBehavior.h 17 Oct 2004 01:16:10 -0000 1.3 @@ -0,0 +1,97 @@ +//-*-c++-*- +#ifndef INCLUDED_MotionStressTestBehavior_h_ +#define INCLUDED_MotionStressTestBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/MotionManager.h" +#include "Motion/MotionSequenceMC.h" +#include + + +//! uses a separate MotionCommand for each of several joints to test for region leaks +class MotionStressTestBehavior : public BehaviorBase { +public: + //! constructor + MotionStressTestBehavior() : BehaviorBase(), nextLeg(RBkLegOrder), curMotions() {} + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + + SharedObject > ms; + ms->setPlayTime(3000); + ms->setOutputCmd(LFrLegOffset+ElevatorOffset,outputRanges[LFrLegOffset+ElevatorOffset][MaxRange]); + ms->setOutputCmd(RFrLegOffset+ElevatorOffset,outputRanges[RFrLegOffset+ElevatorOffset][MaxRange]); + ms->setOutputCmd(LBkLegOffset+ElevatorOffset,outputRanges[LBkLegOffset+ElevatorOffset][MaxRange]); + ms->setOutputCmd(RBkLegOffset+ElevatorOffset,outputRanges[RBkLegOffset+ElevatorOffset][MaxRange]); + ms->setOutputCmd(LFrLegOffset+KneeOffset,0); + ms->setOutputCmd(RFrLegOffset+KneeOffset,0); + ms->setOutputCmd(LBkLegOffset+KneeOffset,0); + ms->setOutputCmd(RBkLegOffset+KneeOffset,0); + for(unsigned int i=HeadOffset; isetOutputCmd(i,0); + MotionManager::MC_ID id=motman->addPrunableMotion(ms); + curMotions.push(id); + cout << get_time() << "\tAdded id " << id << endl; + addMS(LFrLegOrder,3000); + addMS(RFrLegOrder,4000); + addMS(LBkLegOrder,5000); + erouter->addListener(this,EventBase::motmanEGID); + } + + virtual void DoStop() { + erouter->removeListener(this); + while(!curMotions.empty()) { + motman->removeMotion(curMotions.front()); + curMotions.pop(); + } + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e.getTypeID()==EventBase::deactivateETID) { + if(e.getSourceID()!=curMotions.front()) { + cout << e.getSourceID() << " is not mine or is out of order" << endl; + } else { + curMotions.pop(); + } + cout << get_time() << "\t Removed id " << e.getSourceID() << endl; + addMS(nextLeg,3000); + nextLeg=static_cast((nextLeg+1)%4); + } + } + + void addMS(LegOrder_t leg,unsigned int delay=0) { + unsigned int index=leg*JointsPerLeg+RotatorOffset; + SharedObject > ms; + ms->setPlayTime(delay); + ms->setOutputCmd(index,outputRanges[index][MaxRange]); + ms->setPlayTime(delay+2000); + ms->setOutputCmd(index,outputRanges[index][MinRange]); + ms->setPlayTime(delay+4000); + ms->setOutputCmd(index,outputRanges[index][MaxRange]); + MotionManager::MC_ID id=motman->addPrunableMotion(ms); + curMotions.push(id); + cout << get_time() << "\tAdded id " << id << endl; + } + + virtual std::string getName() const { return "MotionStressTestBehavior"; } + + static std::string getClassDescription() { return "uses a separate MotionCommand for each of several joints to test for region leaks"; } + +protected: + LegOrder_t nextLeg; + std::queue curMotions; +}; + +/*! @file + * @brief Defines MotionStressTestBehavior, which uses a separate MotionCommand for each of several joints to test for region leaks + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.3 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Demos/RelaxBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/RelaxBehavior.h,v retrieving revision 1.3 retrieving revision 1.5 diff -u -d -r1.3 -r1.5 --- Behaviors/Demos/RelaxBehavior.h 18 Jan 2004 10:16:56 -0000 1.3 +++ Behaviors/Demos/RelaxBehavior.h 17 Oct 2004 01:16:10 -0000 1.5 @@ -27,13 +27,13 @@ // You could also change the priority level so that anytime // a joint is not in use it goes limp (try kBackgroundPriority) - pidMCID=motman->addMotion(pidMC,MotionManager::kHighPriority,false); + pidMCID=motman->addPersistentMotion(pidMC,MotionManager::kHighPriority); // the 'false' is to keep it from auto-pruning } virtual void DoStop() { motman->removeMotion(pidMCID); //this "one-shot" version of doing things will restore the PIDs - motman->addMotion(SharedObject(1)); + motman->addPrunableMotion(SharedObject(1)); BehaviorBase::DoStop(); } virtual std::string getName() const { return "RelaxBehavior"; } @@ -50,7 +50,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/SimpleChaseBallBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/SimpleChaseBallBehavior.h,v retrieving revision 1.6 retrieving revision 1.9 diff -u -d -r1.6 -r1.9 --- Behaviors/Demos/SimpleChaseBallBehavior.h 5 Dec 2003 20:26:34 -0000 1.6 +++ Behaviors/Demos/SimpleChaseBallBehavior.h 17 Oct 2004 01:16:10 -0000 1.9 @@ -27,13 +27,13 @@ //! adds a headpointer and a walker, and a listens for vision events virtual void DoStart() { BehaviorBase::DoStart(); - walker_id = motman->addMotion(SharedObject()); + walker_id = motman->addPersistentMotion(SharedObject()); //the 'false' prevents autopruning erouter->addListener(this,EventBase::visObjEGID); } //! removes motion commands and stops listening virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); motman->removeMotion(walker_id); BehaviorBase::DoStop(); } @@ -61,9 +61,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.9 $ * $State: Exp $ - * $Date: 2003/12/05 20:26:34 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/StareAtBallBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/StareAtBallBehavior.cc,v retrieving revision 1.6 retrieving revision 1.9 diff -u -d -r1.6 -r1.9 --- Behaviors/Demos/StareAtBallBehavior.cc 9 Dec 2003 17:33:14 -0000 1.6 +++ Behaviors/Demos/StareAtBallBehavior.cc 17 Oct 2004 01:16:10 -0000 1.9 @@ -10,12 +10,12 @@ void StareAtBallBehavior::DoStart() { BehaviorBase::DoStart(); - headpointer_id = motman->addMotion(SharedObject()); + headpointer_id = motman->addPersistentMotion(SharedObject()); erouter->addListener(this,EventBase::visObjEGID,ProjectInterface::visPinkBallSID); } void StareAtBallBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); motman->removeMotion(headpointer_id); BehaviorBase::DoStop(); } Index: Behaviors/Demos/StareAtPawBehavior.h =================================================================== RCS file: Behaviors/Demos/StareAtPawBehavior.h diff -N Behaviors/Demos/StareAtPawBehavior.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/StareAtPawBehavior.h 17 Oct 2004 01:16:10 -0000 1.5 @@ -0,0 +1,102 @@ +//-*-c++-*- +#ifndef INCLUDED_StareAtPawBehavior_h_ +#define INCLUDED_StareAtPawBehavior_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/OldKinematics.h" +#include "Motion/PIDMC.h" +#include "Motion/HeadPointerMC.h" +#include "Motion/MotionManager.h" +#include "Shared/SharedObject.h" + + +//! Uses kinematics to track the paw which last received a button press with the camera +class StareAtPawBehavior : public BehaviorBase { +public: + //! constructor + StareAtPawBehavior() + : BehaviorBase(), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID) + { } + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + for(unsigned int i=0; ioutputs[HeadOffset+i]; + pointID=motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::sensorEGID); + erouter->addListener(this,EventBase::buttonEGID); + } + + virtual void DoStop() { + motman->removeMotion(pointID); + pointID=MotionManager::invalid_MC_ID; + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::buttonEGID) { + + if(e.getSourceID()==LFrPawOffset) + lastLeg=LFrLegOrder; + else if(e.getSourceID()==RFrPawOffset) + lastLeg=RFrLegOrder; + else + return; + if(e.getTypeID()==EventBase::activateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0); + motman->addPrunableMotion(relaxLeg); + } else if(e.getTypeID()==EventBase::deactivateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1); + motman->addPrunableMotion(tightLeg); + } + + } else if(e.getGeneratorID()==EventBase::sensorEGID) { + + double leg_angles[JointsPerLeg]; + for(unsigned int i=0; ioutputs[LegOffset+lastLeg*JointsPerLeg+i]; + vector3d paw; + GetLegPosition(paw,leg_angles,lastLeg); + paw-=body_to_neck; //target should be neck-relative + paw.z+=body_to_neck.z; // but body-height relative (#@%&$) + GetHeadAngles(head_angles,paw,0,0); + /*{ + double tmp[JointsPerLeg]; + GetLegAngles(tmp,paw,lastLeg); + sout->printf("Leg: (%g,%g,%g) (%g,%g,%g) (%g,%g,%g)\n",leg_angles[0],leg_angles[1],leg_angles[2],paw.x,paw.y,paw.z,tmp[0],tmp[1],tmp[2]); + vector3d tmp2; + tmp2=RunForwardModel(head_angles,0,0,vector3d(0,0,0)); + sout->printf("Head: (%g,%g,%g) (%g,%g,%g) (%g,%g,%g)\n",tmp2.x,tmp2.y,tmp2.z,head_angles[0],head_angles[1],head_angles[2],state->outputs[HeadOffset+0],state->outputs[HeadOffset+1],state->outputs[HeadOffset+2]); + }*/ + MMAccessor(pointID)->setJoints(head_angles[0],head_angles[1],head_angles[2]); + + } else { + serr->printf("StareAtPawBehavior: Unhandled event %s\n",e.getName().c_str()); + } + } + + virtual std::string getName() const { return "StareAtPawBehavior"; } + + static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; } + +protected: + LegOrder_t lastLeg; + MotionManager::MC_ID pointID; + double head_angles[JointsPerLeg]; +}; + +/*! @file + * @brief Defines StareAtPawBehavior, which uses kinematics to track the paw which last received a button press with the camera + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.5 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Demos/StareAtPawBehavior2.h =================================================================== RCS file: Behaviors/Demos/StareAtPawBehavior2.h diff -N Behaviors/Demos/StareAtPawBehavior2.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Demos/StareAtPawBehavior2.h 17 Oct 2004 01:32:24 -0000 1.12 @@ -0,0 +1,102 @@ +//-*-c++-*- +#ifndef INCLUDED_StareAtPawBehavior2_h_ +#define INCLUDED_StareAtPawBehavior2_h_ + +#include "Behaviors/BehaviorBase.h" +#include "Motion/PIDMC.h" +#include "Motion/HeadPointerMC.h" +#include "Motion/PostureMC.h" +#include "Motion/MotionManager.h" +#include "Shared/SharedObject.h" +#include "Motion/roboop/robot.h" +#include "Shared/Config.h" +#include "Motion/Kinematics.h" + + +//! Uses kinematics to track the paw which last received a button press with the camera +class StareAtPawBehavior2 : public BehaviorBase { +public: + //! constructor + StareAtPawBehavior2() + : BehaviorBase(), lastLeg(LFrLegOrder), pointID(MotionManager::invalid_MC_ID) + { } + + virtual void DoStart() { + BehaviorBase::DoStart(); // do this first + pointID=motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::sensorEGID); + erouter->addListener(this,EventBase::buttonEGID); + if(state->robotDesign == WorldState::ERS7Mask) + cout << "NOTICE: The ERS-7 has a rather \"sticky\" nod joint\n" + << "(the upper tilt joint). This can cause it to hesitate\n" + << "or altogether fail to precisely center the target position\n" + << "vertically in the center of the image...\n" << endl; + } + + virtual void DoStop() { + motman->removeMotion(pointID); + pointID=MotionManager::invalid_MC_ID; + erouter->removeListener(this); + BehaviorBase::DoStop(); // do this last + } + + virtual void processEvent(const EventBase& e) { + if(e.getGeneratorID()==EventBase::buttonEGID) { + + if(e.getSourceID()==LFrPawOffset) { + lastLeg=LFrLegOrder; + } else if(e.getSourceID()==RFrPawOffset) { + lastLeg=RFrLegOrder; + } else + return; + if(e.getTypeID()==EventBase::activateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject relaxLeg(lastlegoff,lastlegoff+JointsPerLeg,0); + motman->addPrunableMotion(relaxLeg); + } else if(e.getTypeID()==EventBase::deactivateETID) { + unsigned int lastlegoff=LegOffset+lastLeg*JointsPerLeg; + SharedObject tightLeg(lastlegoff,lastlegoff+JointsPerLeg,1); + motman->addPrunableMotion(tightLeg); + } + + } else if(e.getGeneratorID()==EventBase::sensorEGID) { + + //Find paw location + const char * ipname=(lastLeg==LFrLegOrder?"ToeLFrPaw":"ToeRFrPaw"); + NEWMAT::ColumnVector Pobj=kine->getFrameInterestPoint(BaseFrameOffset,ipname); + + //Compute neck angles + PostureEngine pose; + NEWMAT::ColumnVector Plink(4); Plink=0; Plink(3)=1; //infinite ray along z axis, maximizes distance from camera to objective + //NEWMAT::ColumnVector Plink(4); Plink=0; Plink(3)=80; Plink(4)=1; //keep head 8cm away from paw + //Alternatively, could also use the pack function a la: Plink=Kinematics::pack(0,0,80); + pose.solveLinkVector(Pobj,CameraFrameOffset,Plink); + + //Set joint values + MMAccessor(pointID)->setJoints(pose(HeadOffset+0).value,pose(HeadOffset+1).value,pose(HeadOffset+2).value); + } else { + serr->printf("StareAtPawBehavior2: Unhandled event %s\n",e.getName().c_str()); + } + } + + virtual std::string getName() const { return "StareAtPawBehavior2"; } + + static std::string getClassDescription() { return "Uses kinematics to track the paw which last received a button press with the camera"; } + +protected: + LegOrder_t lastLeg; + MotionManager::MC_ID pointID; +}; + +/*! @file + * @brief Defines StareAtPawBehavior2, which uses kinematics to track the paw which last received a button press with the camera + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.12 $ + * $State: Exp $ + * $Date: 2004/10/17 01:32:24 $ + */ + +#endif Index: Behaviors/Demos/ToggleHeadLightBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/ToggleHeadLightBehavior.h,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Behaviors/Demos/ToggleHeadLightBehavior.h 2 Sep 2003 20:58:49 -0000 1.2 +++ Behaviors/Demos/ToggleHeadLightBehavior.h 17 Oct 2004 01:16:10 -0000 1.3 @@ -18,7 +18,7 @@ if(state->robotDesign & WorldState::ERS220Mask) { SharedObject pose; pose->setOutputCmd(ERS220Info::RetractableHeadLEDOffset,true); - light_id=motman->addMotion(pose,false); + light_id=motman->addPersistentMotion(pose); } } @@ -41,9 +41,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ - * $State: Rel $ - * $Date: 2003/09/02 20:58:49 $ + * $Revision: 1.3 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Demos/WalkToTargetMachine.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/WalkToTargetMachine.cc,v retrieving revision 1.10 retrieving revision 1.14 diff -u -d -r1.10 -r1.14 --- Behaviors/Demos/WalkToTargetMachine.cc 5 Dec 2003 20:26:34 -0000 1.10 +++ Behaviors/Demos/WalkToTargetMachine.cc 17 Oct 2004 01:16:10 -0000 1.14 @@ -18,15 +18,21 @@ void WalkToTargetMachine::DoStart() { StateNode::DoStart(); - headpointer_id = motman->addMotion(SharedObject()); - walker_id = motman->addMotion(SharedObject()); + + headpointer_id = motman->addPersistentMotion(SharedObject()); + walker_id = motman->addPersistentMotion(SharedObject()); + erouter->addListener(this,EventBase::visObjEGID,tracking); } void WalkToTargetMachine::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); + motman->removeMotion(headpointer_id); + headpointer_id=MotionManager::invalid_MC_ID; motman->removeMotion(walker_id); + walker_id=MotionManager::invalid_MC_ID; + StateNode::DoStop(); } @@ -77,8 +83,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ + * $Revision: 1.14 $ * $State: Exp $ - * $Date: 2003/12/05 20:26:34 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Demos/WorldStateVelDaemon.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Demos/WorldStateVelDaemon.h,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Behaviors/Demos/WorldStateVelDaemon.h 18 Jan 2004 10:16:56 -0000 1.2 +++ Behaviors/Demos/WorldStateVelDaemon.h 7 Oct 2004 19:07:04 -0000 1.3 @@ -28,7 +28,7 @@ } virtual void DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); // do this last } @@ -87,9 +87,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.3 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/07 19:07:04 $ */ #endif Index: Behaviors/Mon/Aibo3DControllerBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/Aibo3DControllerBehavior.h,v retrieving revision 1.3 retrieving revision 1.5 diff -u -d -r1.3 -r1.5 --- Behaviors/Mon/Aibo3DControllerBehavior.h 18 Jan 2004 10:16:56 -0000 1.3 +++ Behaviors/Mon/Aibo3DControllerBehavior.h 17 Oct 2004 01:16:10 -0000 1.5 @@ -78,7 +78,7 @@ for(unsigned int i=0; ioutputs[i]; // Enable remote control stream - rcontrol_id = motman->addMotion(SharedObject()); + rcontrol_id = motman->addPersistentMotion(SharedObject()); updateRC(); // Turn on wireless cmdsock=wireless->socket(SocketNS::SOCK_STREAM, 2048, 2048); @@ -131,9 +131,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:56 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Mon/EStopControllerBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/EStopControllerBehavior.cc,v retrieving revision 1.3 retrieving revision 1.4 diff -u -d -r1.3 -r1.4 --- Behaviors/Mon/EStopControllerBehavior.cc 14 Jan 2004 20:44:31 -0000 1.3 +++ Behaviors/Mon/EStopControllerBehavior.cc 7 Oct 2004 19:07:04 -0000 1.4 @@ -18,7 +18,7 @@ void EStopControllerBehavior::DoStop() { // Turn off timers - erouter->forgetListener(this); + erouter->removeListener(this); // Close socket; turn wireless off wireless->setDaemon(cmdsock,false); wireless->close(cmdsock); @@ -73,8 +73,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.4 $ * $State: Exp $ - * $Date: 2004/01/14 20:44:31 $ + * $Date: 2004/10/07 19:07:04 $ */ Index: Behaviors/Mon/HeadPointControllerBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/HeadPointControllerBehavior.cc,v retrieving revision 1.4 retrieving revision 1.7 diff -u -d -r1.4 -r1.7 --- Behaviors/Mon/HeadPointControllerBehavior.cc 5 Feb 2004 01:51:39 -0000 1.4 +++ Behaviors/Mon/HeadPointControllerBehavior.cc 17 Oct 2004 01:16:10 -0000 1.7 @@ -1,6 +1,7 @@ #include "HeadPointControllerBehavior.h" #include "Behaviors/Controller.h" #include "Motion/MMAccessor.h" +#include "Motion/HeadPointerMC.h" HeadPointControllerBehavior* HeadPointControllerBehavior::theOne = NULL; @@ -55,7 +56,7 @@ // We listen to timers erouter->addListener(this, EventBase::timerEGID); // Enable head control - head_id = motman->addMotion(SharedObject()); + head_id = motman->addPersistentMotion(SharedObject()); // Turn on wireless theLastOne=theOne; theOne=this; @@ -71,7 +72,7 @@ // Close the GUI Controller::closeGUI("HeadPointGUI"); // Turn off timers - erouter->forgetListener(this); + erouter->removeListener(this); // Close socket; turn wireless off wireless->setDaemon(cmdsock,false); wireless->close(cmdsock); @@ -124,8 +125,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.4 $ + * $Revision: 1.7 $ * $State: Exp $ - * $Date: 2004/02/05 01:51:39 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Mon/HeadPointControllerBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/HeadPointControllerBehavior.h,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Behaviors/Mon/HeadPointControllerBehavior.h 11 Dec 2003 05:49:30 -0000 1.2 +++ Behaviors/Mon/HeadPointControllerBehavior.h 14 Oct 2004 21:59:14 -0000 1.3 @@ -6,7 +6,6 @@ #include "Wireless/Wireless.h" #include "Behaviors/BehaviorBase.h" #include "Motion/MotionManager.h" -#include "Motion/HeadPointerMC.h" #include "Events/EventRouter.h" #include "Events/EventBase.h" #include "Shared/Config.h" @@ -84,9 +83,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.3 $ * $State: Exp $ - * $Date: 2003/12/11 05:49:30 $ + * $Date: 2004/10/14 21:59:14 $ */ #endif Index: Behaviors/Mon/RawCamBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/RawCamBehavior.cc,v retrieving revision 1.10 retrieving revision 1.13 diff -u -d -r1.10 -r1.13 --- Behaviors/Mon/RawCamBehavior.cc 5 Feb 2004 19:11:26 -0000 1.10 +++ Behaviors/Mon/RawCamBehavior.cc 7 Oct 2004 19:07:05 -0000 1.13 @@ -8,17 +8,30 @@ #include "Shared/ProjectInterface.h" RawCamBehavior::RawCamBehavior() - : BehaviorBase(), visRaw(NULL), packet(NULL), cur(NULL), avail(0) + : BehaviorBase(), visRaw(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0) {} void RawCamBehavior::DoStart() { BehaviorBase::DoStart(); - visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, WIRELESS_BUFFER_SIZE); - wireless->setDaemon(visRaw,true); - wireless->listen(visRaw,config->vision.rawcam_port); - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port); + std::vector args; + if(config->vision.rawcam_transport==0) { + max_buf=UDP_WIRELESS_BUFFER_SIZE; + visRaw=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + args.push_back("udp"); + } else if(config->vision.rawcam_transport==1) { + max_buf=TCP_WIRELESS_BUFFER_SIZE; + visRaw=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + wireless->setDaemon(visRaw,true); + args.push_back("tcp"); + } else { + serr->printf("ERROR: Invalid Config::vision.rawcam_transport: %d\n",config->vision.rawcam_transport); + return; + } + wireless->listen(visRaw,config->vision.rawcam_port); + + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","RawVisionGUI",config->vision.rawcam_port,args); erouter->addListener(this,EventBase::visRawCameraEGID,ProjectInterface::visRawCameraSID); erouter->addListener(this,EventBase::visJPEGEGID,ProjectInterface::visColorJPEGSID); @@ -27,12 +40,12 @@ void RawCamBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); Controller::closeGUI("RawVisionGUI"); // this could be considered a bug in our wireless - if we don't setDaemon(...,false) // it will try to listen again even though we explicitly closed the server socket... - wireless->setDaemon(visRaw,false); + wireless->setDaemon(visRaw,false); wireless->close(visRaw->sock); BehaviorBase::DoStop(); } @@ -62,7 +75,7 @@ if(packet!=NULL) return false; - avail=WIRELESS_BUFFER_SIZE-1; //not sure why -1, but Alok had it, so i will too + avail=max_buf-1; //not sure why -1, but Alok had it, so i will too ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRaw->getWriteBuffer(avail); @@ -253,11 +266,19 @@ return true; } +//#include "Shared/WorldState.h" + void RawCamBehavior::closePacket() { if(packet==NULL) return; visRaw->write(cur-packet); + /* cout << "used=" << (cur-packet) << "\tavail==" << avail; + if(state->robotDesign & WorldState::ERS7Mask) + cout << "\tmax bandwidth=" << (cur-packet)/1024.f*30 << "KB/sec" << endl; + else + cout << "\tmax bandwidth=" << (cur-packet)/1024.f*24 << "KB/sec" << endl; + */ packet=cur=NULL; avail=0; } @@ -269,8 +290,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ + * $Revision: 1.13 $ * $State: Exp $ - * $Date: 2004/02/05 19:11:26 $ + * $Date: 2004/10/07 19:07:05 $ */ Index: Behaviors/Mon/RawCamBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/RawCamBehavior.h,v retrieving revision 1.8 retrieving revision 1.9 diff -u -d -r1.8 -r1.9 --- Behaviors/Mon/RawCamBehavior.h 5 Feb 2004 23:33:41 -0000 1.8 +++ Behaviors/Mon/RawCamBehavior.h 28 Sep 2004 22:12:30 -0000 1.9 @@ -33,7 +33,8 @@ //! constructor RawCamBehavior(); - static const unsigned int WIRELESS_BUFFER_SIZE=200000; //!< 200000 bytes for use up to 416x320 + 2*208x160 (double res Y, full res UV on ERS-7) + static const unsigned int TCP_WIRELESS_BUFFER_SIZE=200000; //!< 200000 bytes for use up to 416x320 + 2*208x160 (double res Y, full res UV on ERS-7) + static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size virtual void DoStart(); @@ -61,6 +62,7 @@ char * packet; //!< point to the current buffer being prepared to be sent char * cur; //!< current location within that buffer unsigned int avail; //!< the number of bytes remaining in the buffer + unsigned int max_buf; //!< the buffer size requested from Wireless when the socket was allocated private: RawCamBehavior(const RawCamBehavior&); //!< don't call @@ -73,9 +75,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.8 $ + * $Revision: 1.9 $ * $State: Exp $ - * $Date: 2004/02/05 23:33:41 $ + * $Date: 2004/09/28 22:12:30 $ */ #endif Index: Behaviors/Mon/SegCamBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/SegCamBehavior.cc,v retrieving revision 1.6 retrieving revision 1.9 diff -u -d -r1.6 -r1.9 --- Behaviors/Mon/SegCamBehavior.cc 5 Feb 2004 19:11:26 -0000 1.6 +++ Behaviors/Mon/SegCamBehavior.cc 7 Oct 2004 19:07:05 -0000 1.9 @@ -8,18 +8,31 @@ #include "Vision/RLEGenerator.h" SegCamBehavior::SegCamBehavior() - : BehaviorBase(), visRLE(NULL), packet(NULL), cur(NULL), avail(0) + : BehaviorBase(), visRLE(NULL), packet(NULL), cur(NULL), avail(0), max_buf(0) { } void SegCamBehavior::DoStart() { BehaviorBase::DoStart(); - visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, WIRELESS_BUFFER_SIZE); - wireless->setDaemon(visRLE,true); - wireless->listen(visRLE,config->vision.rle_port); + + std::vector args; + if(config->vision.rle_transport==0) { + max_buf=UDP_WIRELESS_BUFFER_SIZE; + visRLE=wireless->socket(SocketNS::SOCK_DGRAM, 1024, max_buf); + args.push_back("udp"); + } else if(config->vision.rle_transport==1) { + max_buf=TCP_WIRELESS_BUFFER_SIZE; + visRLE=wireless->socket(SocketNS::SOCK_STREAM, 1024, max_buf); + wireless->setDaemon(visRLE,true); + args.push_back("tcp"); + } else { + serr->printf("ERROR: Invalid Config::vision.rle_transport: %d\n",config->vision.rle_transport); + return; + } + wireless->listen(visRLE,config->vision.rle_port); - Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.rle_port); + Controller::loadGUI("org.tekkotsu.mon.VisionGUI","SegVisionGUI",config->vision.rle_port,args); erouter->addListener(this,EventBase::visSegmentEGID,ProjectInterface::visSegmentSID); erouter->addListener(this,EventBase::visRLEEGID,ProjectInterface::visRLESID); @@ -27,12 +40,12 @@ void SegCamBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); Controller::closeGUI("SegVisionGUI"); // this could be considered a bug in our wireless - if we don't setDaemon(...,false) // it will try to listen again even though we explicitly closed the server socket... - wireless->setDaemon(visRLE,false); + wireless->setDaemon(visRLE,false); wireless->close(visRLE->sock); BehaviorBase::DoStop(); } @@ -58,7 +71,7 @@ if(packet!=NULL) return false; - avail=WIRELESS_BUFFER_SIZE-1; //not sure why -1, but Alok had it, so i will too + avail=max_buf-1; //not sure why -1, but Alok had it, so i will too ASSERT(cur==NULL,"cur non-NULL"); cur=NULL; char * buf=packet=(char*)visRLE->getWriteBuffer(avail); @@ -155,8 +168,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.9 $ * $State: Exp $ - * $Date: 2004/02/05 19:11:26 $ + * $Date: 2004/10/07 19:07:05 $ */ Index: Behaviors/Mon/SegCamBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/SegCamBehavior.h,v retrieving revision 1.5 retrieving revision 1.6 diff -u -d -r1.5 -r1.6 --- Behaviors/Mon/SegCamBehavior.h 5 Feb 2004 23:33:41 -0000 1.5 +++ Behaviors/Mon/SegCamBehavior.h 28 Sep 2004 22:12:30 -0000 1.6 @@ -44,7 +44,8 @@ //! constructor SegCamBehavior(); - static const unsigned int WIRELESS_BUFFER_SIZE=85000; //!< 85000 bytes for use up to 416x320 pixels / 8 min expected runs * 5 bytes per run + some padding + static const unsigned int TCP_WIRELESS_BUFFER_SIZE=85000; //!< 85000 bytes for use up to 416x320 pixels / 8 min expected runs * 5 bytes per run + some padding + static const unsigned int UDP_WIRELESS_BUFFER_SIZE=64*1024; //!< 64KB is the max udp packet size virtual void DoStart(); @@ -72,6 +73,7 @@ char * packet; //!< buffer being filled out to be sent char * cur; //!< current location in #packet unsigned int avail; //!< number of bytes remaining in #packet + unsigned int max_buf; //!< the buffer size requested from Wireless when the socket was allocated private: SegCamBehavior(const SegCamBehavior&); //!< don't call @@ -84,9 +86,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.5 $ + * $Revision: 1.6 $ * $State: Exp $ - * $Date: 2004/02/05 23:33:41 $ + * $Date: 2004/09/28 22:12:30 $ */ #endif Index: Behaviors/Mon/ViewWMVarsBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/ViewWMVarsBehavior.h,v retrieving revision 1.1 retrieving revision 1.2 diff -u -d -r1.1 -r1.2 --- Behaviors/Mon/ViewWMVarsBehavior.h 10 Oct 2003 00:47:52 -0000 1.1 +++ Behaviors/Mon/ViewWMVarsBehavior.h 16 Apr 2004 20:17:22 -0000 1.2 @@ -7,7 +7,7 @@ #include "Shared/Config.h" #include -//! simply launches the Watchable Memory GUI +//! simply launches the Watchable Memory GUI, which should connect to the already-running WMMonitorBehavior class ViewWMVarsBehavior : public BehaviorBase { public: //! constructor @@ -29,19 +29,19 @@ static std::string getClassDescription() { char tmp[20]; sprintf(tmp,"%d",config->main.wmmonitor_port); - return std::string("Brings up the WatchableMemory GUI on port ")+tmp; + return std::string("Brings up the WatchableMemory GUI on port ")+tmp+std::string(" (connects to WMMonitorBehavior, this just launches the GUI)"); } }; /*! @file - * @brief Describes ViewWMVarsBehavior, simply launches the Watchable Memory GUI + * @brief Defines ViewWMVarsBehavior, simply launches the Watchable Memory GUI, which should connect to the already-running WMMonitorBehavior * @author ejt (Creator) * * $Author: ejt $ * $Name: $ - * $Revision: 1.1 $ - * $State: Rel $ - * $Date: 2003/10/10 00:47:52 $ + * $Revision: 1.2 $ + * $State: Exp $ + * $Date: 2004/04/16 20:17:22 $ */ #endif Index: Behaviors/Mon/WMMonitorBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/WMMonitorBehavior.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Behaviors/Mon/WMMonitorBehavior.cc 19 Jan 2004 20:35:31 -0000 1.2 +++ Behaviors/Mon/WMMonitorBehavior.cc 16 Apr 2004 20:17:22 -0000 1.3 @@ -26,15 +26,20 @@ wmitem->toString()); } else if (cmd[0]=='r') { WMregistry* wmreg=NULL; - if (var.length()==0) wmreg=&GlobalWM; + if (var.length()==0) + wmreg=&GlobalWM; else { WMitem* wmitem=static_cast *> (find (var)); if (wmitem!=NULL) wmreg=&wmitem->get_value(); + else + serr->printf("WMMonitorBehavior: Could not find '%s'\n",var.c_str()); } - if (wmreg!=NULL) - for (std::vector::const_iterator it = wmreg->entries.begin(); - it != wmreg->entries.end(); it++) { + if (wmreg==NULL) + serr->printf("WMMonitorBehavior: wmreg is NULL\n"); + else { + //sout->printf("Reporting:\n"); + for (std::vector::const_iterator it = wmreg->entries.begin(); it != wmreg->entries.end(); it++) { WMentry* entry=*it; std::string sn(entry->item_name); WMregistry *temp=entry->registry; @@ -42,8 +47,10 @@ sn=temp->name + "." + sn; temp=temp->parent; } + //sout->printf("Reporting %s %s %s\n",entry->type_name.c_str(),sn.c_str(),entry->item->toString().c_str()); report(entry->type_name, sn, entry->item->toString()); } + } } else if (cmd[0]=='d') { // set debug mode (blocking/nonblocking) // implement within this class } @@ -98,9 +105,7 @@ WMMonitorBehavior::report (const char* var_type, int var_type_length, const char* var_name, int var_name_length, const char* value, int value_length) { - char *buf=(char*)cmdsock->getWriteBuffer(5+ var_type_length+ - var_name_length+ - value_length); + char *buf=(char*)cmdsock->getWriteBuffer(5*sizeof(int)+var_type_length+var_name_length+value_length); if (buf) { encodeHeader(&buf, packet_wmclass); encode(&buf, var_type_length); @@ -110,7 +115,8 @@ encode(&buf, value_length); encode(&buf, value, value_length); cmdsock->write(5*sizeof(int)+var_type_length+var_name_length+value_length); - } + } else + serr->printf("WMMonitorBehavior: Failed to get write buffer\n"); } int wmmonitorcmd_callback(char *buf, int bytes) { @@ -141,7 +147,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.3 $ * $State: Exp $ - * $Date: 2004/01/19 20:35:31 $ + * $Date: 2004/04/16 20:17:22 $ */ Index: Behaviors/Mon/WMMonitorBehavior.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/WMMonitorBehavior.h,v retrieving revision 1.6 retrieving revision 1.8 diff -u -d -r1.6 -r1.8 --- Behaviors/Mon/WMMonitorBehavior.h 19 Jan 2004 20:35:31 -0000 1.6 +++ Behaviors/Mon/WMMonitorBehavior.h 7 Oct 2004 19:07:05 -0000 1.8 @@ -77,7 +77,7 @@ virtual void DoStop() { // Controller::closeGUI(getGUIType()); - erouter->forgetListener(this); + erouter->removeListener(this); // Close socket; turn wireless off wireless->setDaemon(cmdsock,false); wireless->close(cmdsock); @@ -106,7 +106,7 @@ } protected: - static const unsigned int packet_wmclass=12; //!< magic id number + static const unsigned int packet_wmclass=14; //!< magic id number, corresponds to Listener.java PACKET_WMCLASS //! writes packet type and timestamp inline static void encodeHeader(char **dst, unsigned int pformat) { encode(dst, pformat); @@ -137,9 +137,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.8 $ * $State: Exp $ - * $Date: 2004/01/19 20:35:31 $ + * $Date: 2004/10/07 19:07:05 $ */ #endif Index: Behaviors/Mon/WalkControllerBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/WalkControllerBehavior.cc,v retrieving revision 1.3 retrieving revision 1.6 diff -u -d -r1.3 -r1.6 --- Behaviors/Mon/WalkControllerBehavior.cc 19 Jan 2004 19:43:51 -0000 1.3 +++ Behaviors/Mon/WalkControllerBehavior.cc 17 Oct 2004 01:16:10 -0000 1.6 @@ -87,7 +87,7 @@ // We listen to timers erouter->addListener(this, EventBase::timerEGID); // Enable walker (the MC_ID can be accessed through the shared_walker later) - motman->addMotion(shared_walker); + motman->addPersistentMotion(shared_walker); // Turn on wireless theLastOne=theOne; theOne=this; @@ -103,7 +103,7 @@ // Close the GUI Controller::closeGUI("WalkGUI"); // Turn off timers - erouter->forgetListener(this); + erouter->removeListener(this); // Close socket; turn wireless off wireless->setDaemon(cmdsock,false); wireless->close(cmdsock); @@ -158,8 +158,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.6 $ * $State: Exp $ - * $Date: 2004/01/19 19:43:51 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Behaviors/Mon/WorldStateSerializerBehavior.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Mon/WorldStateSerializerBehavior.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Behaviors/Mon/WorldStateSerializerBehavior.cc 24 Jan 2004 21:28:50 -0000 1.2 +++ Behaviors/Mon/WorldStateSerializerBehavior.cc 7 Oct 2004 19:07:05 -0000 1.3 @@ -21,7 +21,7 @@ } void WorldStateSerializerBehavior::DoStop() { - erouter->forgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); // do this last } @@ -54,7 +54,7 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.3 $ * $State: Exp $ - * $Date: 2004/01/24 21:28:50 $ + * $Date: 2004/10/07 19:07:05 $ */ Index: Behaviors/Nodes/HeadPointerNode.h =================================================================== RCS file: Behaviors/Nodes/HeadPointerNode.h diff -N Behaviors/Nodes/HeadPointerNode.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Nodes/HeadPointerNode.h 17 Oct 2004 01:16:10 -0000 1.2 @@ -0,0 +1,60 @@ +//-*-c++-*- +#ifndef INCLUDED_HeadPointerNode_h_ +#define INCLUDED_HeadPointerNode_h_ + +#include "Behaviors/StateNode.h" +#include "Events/EventRouter.h" +#include "Motion/HeadPointerMC.h" + +//! A simple StateNode that executes a LedMC motion command and throws a status event upon completion +class HeadPointerNode : public StateNode { +protected: + SharedObject head_mc; //!< MotionCommand used by this node + MotionManager::MC_ID head_id; //!< id number for the MotionCommand + +public: + //! constructor + HeadPointerNode(std::string nodename="HeadPointerNode") : + StateNode(nodename), head_mc(), head_id(MotionManager::invalid_MC_ID) {} + + //! activate the node + virtual void DoStart() { + head_id = motman->addPersistentMotion(head_mc); + erouter->addListener(this,EventBase::motmanEGID,head_id,EventBase::statusETID); + StateNode::DoStart(); // don't activate transitions until our listener has been added + } + + //! deactivate the node + virtual void DoStop() { + motman->removeMotion(head_id); + head_id = MotionManager::invalid_MC_ID; + erouter->removeListener(this); + StateNode::DoStop(); + } + + //! receive motmanEGID status event and throw stateMachineEGID status event + virtual void processEvent(const EventBase&) { + erouter->postEvent(EventBase::stateMachineEGID,(unsigned int)this,EventBase::statusETID,0); + } + + //! reveal the MotionCommand + SharedObject& getMC() { return head_mc; } + + //! reveal the MC_ID + MotionManager::MC_ID& getMC_ID() { return head_id; } + + +}; + +/*! @file + * @brief Defines HeadPointerNode, a simple StateNode that runs a HeadPointerMC motion command and throws a status event upon completion + * @author dst (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.2 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Nodes/PlayMotionSequenceNode.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Nodes/PlayMotionSequenceNode.h,v retrieving revision 1.2 retrieving revision 1.6 diff -u -d -r1.2 -r1.6 --- Behaviors/Nodes/PlayMotionSequenceNode.h 18 Jan 2004 10:16:57 -0000 1.2 +++ Behaviors/Nodes/PlayMotionSequenceNode.h 17 Oct 2004 01:16:10 -0000 1.6 @@ -35,7 +35,7 @@ virtual void DoStop() { //std::cout << "PlayMotionSequenceNode::DoStop(); " << std::endl; - erouter->forgetListener(this); + erouter->removeListener(this); motman->removeMotion(msid); msid=MotionManager::invalid_MC_ID; StateNode::DoStop(); @@ -78,6 +78,7 @@ if(looping) { updateMS(filename); } + erouter->postEvent(EventBase::stateMachineEGID,(unsigned int)this,EventBase::statusETID,0); } } @@ -97,7 +98,7 @@ //! resets the motion command and starts it playing void updateMS(const std::string& file) { if(msid==MotionManager::invalid_MC_ID) { - msid=motman->addMotion(SharedObject >(file.c_str())); + msid=motman->addPrunableMotion(SharedObject >(file.c_str())); msidIsMine=true; } else { MMAccessor > ms(msid); @@ -120,9 +121,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.6 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:57 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Nodes/SoundNode.h =================================================================== RCS file: Behaviors/Nodes/SoundNode.h diff -N Behaviors/Nodes/SoundNode.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Nodes/SoundNode.h 8 Oct 2004 00:08:47 -0000 1.5 @@ -0,0 +1,63 @@ +//-*-c++-*- +#ifndef INCLUDED_SoundNode_h_ +#define INCLUDED_SoundNode_h_ + +#include "Behaviors/StateNode.h" +#include "Events/EventRouter.h" +#include "SoundPlay/SoundManager.h" + +//! A simple StateNode that plays a sound upon startup and throws a status event on completion +class SoundNode : public StateNode { +protected: + std::string filename; + SoundManager::Play_ID curplay_id; + +public: + //! constructor + SoundNode(std::string nodename="SoundNode", std::string soundfilename="") : + StateNode(nodename), filename(soundfilename), curplay_id(SoundManager::invalid_Play_ID) {} + + //! activate the node + virtual void DoStart() { + curplay_id = sndman->PlayFile(filename); + erouter->addListener(this,EventBase::audioEGID,curplay_id,EventBase::deactivateETID); + StateNode::DoStart(); // don't activate transitions until our listener has been added + } + + //! deactivate the node + virtual void DoStop() { + erouter->removeListener(this); + StateNode::DoStop(); + } + + //! receive audioEGID status event and throw stateMachineEGID status event + virtual void processEvent(const EventBase&) { + erouter->postEvent(EventBase::stateMachineEGID,(unsigned int)this,EventBase::statusETID,0); + } + + //! interrupts playing of the current sound + void StopPlay() { + sndman->StopPlay(curplay_id); + curplay_id = SoundManager::invalid_Play_ID; + } + + //! returns the name of the sound file associated with this node + std::string getFileName() { return filename; } + + //! sets the name of the sound file associated with this node + void setFileName(std::string &soundfilename) { filename = soundfilename; } + +}; + +/*! @file + * @brief Defines SoundNode, a simple StateNode that plays a sound and throws a status event upon completion + * @author dst (Creator) + * + * $Author: dst $ + * $Name: $ + * $Revision: 1.5 $ + * $State: Exp $ + * $Date: 2004/10/08 00:08:47 $ + */ + +#endif Index: Behaviors/Nodes/WalkNode.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Nodes/WalkNode.h,v retrieving revision 1.3 retrieving revision 1.5 diff -u -d -r1.3 -r1.5 --- Behaviors/Nodes/WalkNode.h 29 Oct 2003 22:36:20 -0000 1.3 +++ Behaviors/Nodes/WalkNode.h 17 Oct 2004 01:16:10 -0000 1.5 @@ -90,7 +90,7 @@ if(walkid==MotionManager::invalid_MC_ID) { SharedObject walk; walk->setTargetVelocity(xvel,yvel,avel); - walkid=motman->addMotion(walk); + walkid=motman->addPersistentMotion(walk); walkidIsMine=true; } else { MMAccessor walk(walkid); @@ -111,9 +111,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.3 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2003/10/29 22:36:20 $ + * $Date: 2004/10/17 01:16:10 $ */ #endif Index: Behaviors/Transitions/CompareTrans.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transitions/CompareTrans.h,v retrieving revision 1.5 retrieving revision 1.7 diff -u -d -r1.5 -r1.7 --- Behaviors/Transitions/CompareTrans.h 11 Nov 2003 00:08:18 -0000 1.5 +++ Behaviors/Transitions/CompareTrans.h 7 Oct 2004 19:07:05 -0000 1.7 @@ -43,28 +43,28 @@ virtual void DoStart() { Transition::DoStart(); erouter->addListener(this,poller); } //!stops listening - virtual void DoStop() { erouter->forgetListener(this); Transition::DoStop(); } + virtual void DoStop() { erouter->removeListener(this); Transition::DoStop(); } //!don't care about the event, just a pulse to check the values virtual void processEvent(const EventBase&) { switch(tst) { case LT: - if(*monaddListener(this, + EventBase::stateMachineEGID, + (unsigned int)getSources()[i], + EventBase::statusETID); + }; + } + + //! stops listening + virtual void DoStop() { + erouter->removeListener(this); + delete completions; + completions = NULL; + Transition::DoStop(); + } + + //! record completions, and fire the transition if all sources have completed + virtual void processEvent(const EventBase &event) { + int numcomplete = 0; + for ( unsigned int i=0; i 0 ? minsrcs : (int)getSources().size()); + if (numcomplete >= threshold) fire(); + } + + virtual std::string getName() const { return "CompletionTrans"; } + +protected: + //!@name Dummy functions to satisfy the compiler + CompletionTrans(const CompletionTrans&); //!< don't call this + CompletionTrans& operator=(const CompletionTrans&); //!< don't call this + //@} + +}; + +/*! @file + * @brief Defines Completiontrans, which causes a transition if all sources have signalled completion + * @author dst (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2004/10/17 01:16:10 $ + */ + +#endif Index: Behaviors/Transitions/EventTrans.h =================================================================== RCS file: Behaviors/Transitions/EventTrans.h diff -N Behaviors/Transitions/EventTrans.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Transitions/EventTrans.h 7 Oct 2004 19:07:05 -0000 1.3 @@ -0,0 +1,62 @@ +//-*-c++-*- +#ifndef INCLUDED_EventTrans_h_ +#define INCLUDED_EventTrans_h_ + +#include "Behaviors/Transition.h" +#include "Events/EventRouter.h" + +//! causes a transition when the specified event is received + +class EventTrans : public Transition { +private: + int argcount; + EventBase::EventGeneratorID_t egid; + unsigned int esid; + EventBase::EventTypeID_t etid; + +public: + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid) : + Transition(destination), argcount(1), egid(gid), esid(0), etid(EventBase::statusETID) {}; + + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, unsigned int sid) : + Transition(destination), argcount(2), egid(gid), esid(sid), etid(EventBase::statusETID) {}; + + EventTrans(StateNode* destination, EventBase::EventGeneratorID_t gid, + unsigned int sid, EventBase::EventTypeID_t tid) : + Transition(destination), argcount(3), egid(gid), esid(sid), etid(tid) {}; + + //! starts listening + virtual void DoStart() { + Transition::DoStart(); + switch (argcount) { + case 1: erouter->addListener(this,egid); break; + case 2: erouter->addListener(this,egid,esid); break; + case 3: erouter->addListener(this,egid,esid,etid); + }; + } + + //! stops listening + virtual void DoStop() { + erouter->removeListener(this); + Transition::DoStop(); + } + + //! fire the transition if an event is seen + virtual void processEvent(const EventBase&) { fire(); } + + virtual std::string getName() const { return "EventTrans"; } + +}; + +/*! @file + * @brief Defines EventTrans, which causes a transition if an event of the specified type occurs + * @author dst (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.3 $ + * $State: Exp $ + * $Date: 2004/10/07 19:07:05 $ + */ + +#endif Index: Behaviors/Transitions/NullTrans.h =================================================================== RCS file: Behaviors/Transitions/NullTrans.h diff -N Behaviors/Transitions/NullTrans.h --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Behaviors/Transitions/NullTrans.h 7 Oct 2004 19:07:05 -0000 1.3 @@ -0,0 +1,41 @@ +//-*-c++-*- +#ifndef INCLUDED_NullTrans_h_ +#define INCLUDED_NullTrans_h_ + +#include "Behaviors/Transition.h" +#include "Events/EventRouter.h" + +//! a transition that occurs (via a 0 msec Timer event) as soon as the source node finishes starting up +class NullTrans : public Transition { +public: + //! constructor + NullTrans(StateNode* destination) : Transition(destination) {} + + //!starts 0 msec timer, so transition will occur very soon + virtual void DoStart() { + Transition::DoStart(); + erouter->addTimer(this,0,0,false); + } + + //!stops timer + virtual void DoStop() { erouter->removeListener(this); Transition::DoStop(); } + + //!when timer event is received, fire() the transition + virtual void processEvent(const EventBase&) { fire(); } + + virtual std::string getName() const { return "TimeOutTrans"; } + +}; + +/*! @file + * @brief Defines NullTrans, which causes a transition as soon as the source node finishes starting up + * @author dst (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.3 $ + * $State: Exp $ + * $Date: 2004/10/07 19:07:05 $ + */ + +#endif Index: Behaviors/Transitions/TimeOutTrans.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transitions/TimeOutTrans.h,v retrieving revision 1.9 retrieving revision 1.12 diff -u -d -r1.9 -r1.12 --- Behaviors/Transitions/TimeOutTrans.h 11 Nov 2003 00:08:18 -0000 1.9 +++ Behaviors/Transitions/TimeOutTrans.h 7 Oct 2004 19:07:05 -0000 1.12 @@ -8,35 +8,35 @@ //! causes a transition after a specified amount of time has passed class TimeOutTrans : public Transition { public: - //! constructor, specify delay in milliseconds - TimeOutTrans(StateNode* destination, unsigned int delay) : Transition(destination), d(delay) {} - - //!starts timer - virtual void DoStart() { - Transition::DoStart(); - resetTimer(); - } + //! constructor, specify delay in milliseconds + TimeOutTrans(StateNode* destination, unsigned int delay) : Transition(destination), d(delay) {} - //!stops timer - virtual void DoStop() { erouter->forgetListener(this); Transition::DoStop(); } + //!starts timer + virtual void DoStart() { + Transition::DoStart(); + resetTimer(); + } - //!resets timer - void resetTimer() { - //std::cout << "Reset @ " << get_time() << " stop @ " << get_time()+d << ' ' << this << std::endl; - erouter->addTimer(this,0,d,false); - } + //!stops timer + virtual void DoStop() { erouter->removeListener(this); Transition::DoStop(); } + + //!resets timer + void resetTimer() { + // std::cout << "Reset @ " << get_time() << " stop @ " << get_time()+d << ' ' << this << std::endl; + erouter->addTimer(this,0,d,false); + } - //!if we receive the timer event, activate() - virtual void processEvent(const EventBase& e) { - std::cout << "Timeout @ " << get_time() << " from " << e.getName() << ' ' << this << std::endl; - activate(); - } + //!if we receive the timer event, fire() + virtual void processEvent(const EventBase&) { + // std::cout << "Timeout @ " << get_time() << " from " << event.getName() << ' ' << this << std::endl; + fire(); + } - virtual std::string getName() const { return "TimeOutTrans"; } + virtual std::string getName() const { return "TimeOutTrans"; } protected: - //!amount to delay (in milliseconds) before transition - unsigned int d; + //!amount to delay (in milliseconds) before transition + unsigned int d; }; /*! @file @@ -45,9 +45,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.12 $ * $State: Exp $ - * $Date: 2003/11/11 00:08:18 $ + * $Date: 2004/10/07 19:07:05 $ */ #endif Index: Behaviors/Transitions/VisualTargetCloseTrans.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transitions/VisualTargetCloseTrans.h,v retrieving revision 1.9 retrieving revision 1.12 diff -u -d -r1.9 -r1.12 --- Behaviors/Transitions/VisualTargetCloseTrans.h 23 Dec 2003 06:33:42 -0000 1.9 +++ Behaviors/Transitions/VisualTargetCloseTrans.h 7 Oct 2004 19:07:05 -0000 1.12 @@ -20,9 +20,9 @@ virtual void DoStart() { Transition::DoStart(); erouter->addListener(this,EventBase::visObjEGID,sid); } //!called by StateNode when it becomes inactive - undo whatever you did in Enable() - virtual void DoStop() { erouter->forgetListener(this); Transition::DoStop(); } + virtual void DoStop() { erouter->removeListener(this); Transition::DoStop(); } - //!if the object is "close", calls activate() + //!if the object is "close", calls fire() virtual void processEvent(const EventBase& e) { const VisionObjectEvent* ve=dynamic_cast(&e); ASSERTRET(ve!=NULL,"Casting error"); @@ -37,8 +37,8 @@ IRDistOffset=ERS220Info::IRDistOffset; else if(state->robotDesign & WorldState::ERS7Mask) IRDistOffset=ERS7Info::NearIRDistOffset; - if(x*x+y*y<0.02f && IRDistOffset!=-1U && state->sensors[IRDistOffset]<300) - activate(); + if(x*x+y*y<0.02f && IRDistOffset!=-1U && state->sensors[IRDistOffset]<225) + fire(); } virtual std::string getName() const { return "VisualTargetCloseTrans"; } @@ -54,9 +54,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.12 $ * $State: Exp $ - * $Date: 2003/12/23 06:33:42 $ + * $Date: 2004/10/07 19:07:05 $ */ #endif Index: Behaviors/Transitions/VisualTargetTrans.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Behaviors/Transitions/VisualTargetTrans.h,v retrieving revision 1.9 retrieving revision 1.11 diff -u -d -r1.9 -r1.11 --- Behaviors/Transitions/VisualTargetTrans.h 8 Dec 2003 00:21:02 -0000 1.9 +++ Behaviors/Transitions/VisualTargetTrans.h 7 Oct 2004 19:07:05 -0000 1.11 @@ -26,13 +26,13 @@ //!called by StateNode when it becomes inactive - undo whatever you did in Enable() virtual void DoStop() { //serr->printf("VisualTargetTrans::DoStop() - enter\n"); - erouter->forgetListener(this); + erouter->removeListener(this); count=0; Transition::DoStop(); //serr->printf("VisualTargetTrans::DoStop() - leave\n"); } - //!if the object is "close", calls activate() + //!if the object is "close", calls fire() virtual void processEvent(const EventBase& e) { //serr->printf("VisualTargetTrans::processEvent() - enter %d\n",get_time()); @@ -41,7 +41,7 @@ else count++; if(count>5) - activate(); + fire(); //serr->printf("VisualTargetTrans::processEvent() - leave %d\n",get_time()); } @@ -61,9 +61,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.11 $ * $State: Exp $ - * $Date: 2003/12/08 00:21:02 $ + * $Date: 2004/10/07 19:07:05 $ */ #endif Index: Events/ButtonEvent.cc =================================================================== RCS file: Events/ButtonEvent.cc diff -N Events/ButtonEvent.cc --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Events/ButtonEvent.cc 16 Sep 2004 20:39:53 -0000 1.1 @@ -0,0 +1,86 @@ +#include "ButtonEvent.h" +#include + +std::string +ButtonEvent::getDescription(bool /*showTypeSpecific=true*/, unsigned int verbosity/*=0*/) const { + std::ostringstream logdata; + logdata << getName(); + char hexstring[30]; + snprintf(hexstring,sizeof(hexstring),"0x%x",getSourceID()); + logdata << '\t' << hexstring; + switch(getTypeID()) { + case EventBase::activateETID: + logdata << "\tA"; break; + case EventBase::statusETID: + logdata << "\tS"; break; + case EventBase::deactivateETID: + logdata << "\tD"; break; + case EventBase::numETIDs: + logdata << "\tU"; break; //UNKNOWN TYPE + } + if(verbosity>=1) + logdata << '\t' << getDuration() << '\t' << getTimeStamp(); + if(verbosity>=2) + logdata << '\t' << getMagnitude(); + return logdata.str(); +} + +unsigned int +ButtonEvent::getBinSize() const { + unsigned int used=0; + used+=creatorSize("ButtonEvent"); + used+=EventBase::getBinSize(); + return used; +} + +unsigned int +ButtonEvent::LoadBuffer(const char buf[], unsigned int len) { + unsigned int origlen=len; + unsigned int used=0; + if(0==(used=checkCreator("ButtonEvent",buf,len,true))) return 0; + len-=used; buf+=used; + if(0==(used=EventBase::LoadBuffer(buf,len))) return 0; + len-=used; buf+=used; + return origlen-len; +} + +unsigned int +ButtonEvent::SaveBuffer(char buf[], unsigned int len) const { + unsigned int origlen=len; + unsigned int used=0; + if(0==(used=saveCreator("ButtonEvent",buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=EventBase::SaveBuffer(buf,len))) return 0; + len-=used; buf+=used; + return origlen-len; +} + +void +ButtonEvent::genName() { + if(!nameisgen) + return; + if(genID +#include const char* const EventBase::EventGeneratorNames[numEGIDs] = { - "UnknownGen", - "AI", - "Audio", - "Button", - "EventRouter", - "EStop", - "Locomotion", - "MotionManager", - "Power", - "Sensor", - "StateMachine", - "TextMsg", - "Timer", - "VisionOFbkVector", - "VisionRawCamera", - "VisionInterleaver", - "VisionJPEGCompressor", - "VisionSegmenter", - "VisionRLEEncoder", - "VisionRegionConnector", - "VisionObjectDetector", - "WMVariable", - "WorldModel", + "unknownEGID", + "aiEGID", + "audioEGID", + "buttonEGID", + "erouterEGID", + "estopEGID", + "locomotionEGID", + "micOSndEGID", + "micRawEGID", + "micFFTEGID", + "motmanEGID", + "powerEGID", + "sensorEGID", + "stateMachineEGID", + "testmsgEGID", + "timerEGID", + "visOFbkEGID", + "visRawCameraEGID", + "visInterleaveEGID", + "visJPEGEGID", + "visSegmentEGID", + "visRLEEGID", + "visRegionEGID", + "visObjEGID", + "wmVarEGID", + "worldModelEGID", }; @@ -50,6 +54,29 @@ } +std::string +EventBase::getDescription(bool /*showTypeSpecific=true*/, unsigned int verbosity/*=0*/) const { + std::ostringstream logdata; + logdata << getName(); + char hexstring[30]; + snprintf(hexstring,sizeof(hexstring),"0x%x",getSourceID()); + logdata << '\t' << hexstring; + switch(getTypeID()) { + case EventBase::activateETID: + logdata << "\tA"; break; + case EventBase::statusETID: + logdata << "\tS"; break; + case EventBase::deactivateETID: + logdata << "\tD"; break; + case EventBase::numETIDs: + logdata << "\tU"; break; //UNKNOWN TYPE + } + if(verbosity>=1) + logdata << '\t' << getDuration() << '\t' << getTimeStamp(); + if(verbosity>=2) + logdata << '\t' << getMagnitude(); + return logdata.str(); +} unsigned int EventBase::getBinSize() const { @@ -142,8 +169,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.15 $ + * $Revision: 1.19 $ * $State: Exp $ - * $Date: 2003/12/05 20:26:42 $ + * $Date: 2004/09/28 23:07:02 $ */ Index: Events/EventBase.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/EventBase.h,v retrieving revision 1.21 retrieving revision 1.27 diff -u -d -r1.21 -r1.27 --- Events/EventBase.h 9 Feb 2004 22:45:28 -0000 1.21 +++ Events/EventBase.h 18 Oct 2004 16:59:20 -0000 1.27 @@ -12,6 +12,10 @@ * you have to do is add a new entry to the ID list (#EventGeneratorID_t) * and then put its name in the #EventGeneratorNames[] array. * + * The #EventGeneratorID_t list should contain links to the + * generators' documentation, or will directly give information about + * what to expect in events from that generator. + * * Alternatively, there is an 'unlicensed spectrum' available under * the #unknownEGID. You can send out events from that generator just * like any other, but it should only be used for quick tests and hacking @@ -23,14 +27,19 @@ * #visObjEGID) refers to seeing a particular object. These SIDs are * usually defined in the generators themselves. * + * If more information needs to be sent along with the event, the + * cleanest solution is to create a subclass of EventBase to hold the + * additional information. For example, you can see the existing + * subclasses in the inheritance diagram above. If you want to use a + * quick hack however, you could assume the pointer size is the same + * as an unsigned int and just pass a pointer to data as the SID. + * Your funeral, err, I mean your call. ;) + * * The #duration field is also generator specific - some may refer to * the time since the last activation event (e.g. button events) * where as others refer to time since last status (e.g. sensors * updates) * - * The #EventGeneratorID_t list should contain links to the - * generators' documentation, or will directly give information about - * what to expect in events from that generator. */ class EventBase : public LoadSave { public: @@ -43,16 +52,19 @@ aiEGID, //!< not being used, yet (might use this when AI makes decisions?) audioEGID, //!< Sends an event when a sound starts/ends playback, status events as chained sounds end; SID is SoundManager::Play_ID; duration is playtime buttonEGID, //!< Sends activate event for button down, deactivate for button up. Status events only for when pressure sensitive buttons' reading changes. (on sensorEGID updates); SIDs are from ButtonOffset_t in the namespace of the target model (e.g. ERS210Info::ButtonOffset_t); duration is button down time - erouterEGID, //!< Sends activate event on first listener, deactivate on last listener, and status on other listener changes.; SID is the generator ID affected (NOT IMPLEMENTED YET) + erouterEGID, //!< Sends activate event on first listener, deactivate on last listener, and status on other listener changes.; SID is the generator ID affected estopEGID, //!< Sends an event when the estop is turned on or off; SID is the MotionManager::MC_ID of the EmergencyStopMC; duration is length of estop activation locomotionEGID, //!< Sends events regarding transportation in the world; you can/should assume these will all be LocomotionEvent classes; SID is MotionManager::MC_ID of posting MotionCommand; duration is the time since last velocity change of that MC. (You could generate these for things other than walking...) - motmanEGID, //!< Sends events when a MotionCommand is added or removed, SID is is the MotionManager::MC_ID, duration is always 0 + micOSndEGID, //!< Sends a DataEvent for every audio buffer received from the system; SID and duration are always 0 (This is generated by the MainObj instantiation of MMCombo) + micRawEGID, //!< reserved for future use + micFFTEGID, //!< reserved for future use + motmanEGID, //!< Sends events when a MotionCommand is added or removed, SID is is the MotionManager::MC_ID, duration is always 0; individual MotionCommands may throw status events to signal intermediary status powerEGID, //!< Sends events for low power warnings, temperature, etc. see PowerSourceID::PowerSourceID_t sensorEGID, //!< Sends a status event when new sensor readings are available. see SensorSourceID::SensorSourceID_t stateMachineEGID, //!< Sends an event upon entering and leaving a StateNode; SID is pointer to the StateNode; duration is always 0 textmsgEGID, //!< Sends status events when a text msg is received on console; generated by the Controller, SID is always -1; durations is always 0 (see Controller for more information) timerEGID, //!< Sends timer events; you set timers explicitly, you don't have to listen as well. (See EventRouter::addTimer()) There's no cross-talk, only the listener which requested the timer will receive it; SID is whatever you requested it to be; duration is the time (since boot, in ms) that the timer was supposed to go off; these are always status - visOFbkEGID, //!< Sends a DataEvent for every camera image received from the system; SID and duration are always 0 (This is generated by the MainObj instantiation of MMCombo) + visOFbkEGID, //!< Sends a DataEvent < OFbkImageVectorData > for every camera image received from the system; SID and duration are always 0 (This is generated by the MainObj instantiation of MMCombo) visRawCameraEGID, //!< Sends a FilterBankEvent when new raw camera images are available; SID is whatever value you gave during setup (typically in StartupBehavior_SetupVision.cc), duration is always 0 visInterleaveEGID,//!< Sends a FilterBankEvent when new interleaved images are available; SID is whatever value you gave during setup (typically in StartupBehavior_SetupVision.cc), duration is always 0 visJPEGEGID, //!< Sends a FilterBankEvent when JPEG compressed images are available; SID is whatever value you gave during setup (typically in StartupBehavior_SetupVision.cc), duration is always 0 @@ -109,6 +121,15 @@ virtual const std::string& resetName() { nameisgen=true; genName(); return stim_id; } //!< resets name to generated form, overwriting any previous name virtual bool isCustomName() const { return !nameisgen; } //!< returns true if not using the generated name + //! generates a description of the event with variable verbosity + /*! @param showTypeSpecific will signal subclasses to add additional information + * @param verbosity can be one of the following values: + * - 0 - the name and type + * - 1 - the name, type, duration, and timestamp + * - 2 and above - the name, type, duration, and magnitude + * if showTypeSpecific, additional fields will be added after the common fields listed above. */ + virtual std::string getDescription(bool showTypeSpecific=true, unsigned int verbosity=0) const; + inline bool operator<(const EventBase& e) const { return timestampforgetListener(this); + erouter->removeListener(this); BehaviorBase::DoStop(); } @@ -20,8 +20,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.1 $ + * $Revision: 1.2 $ * $State: Exp $ - * $Date: 2003/12/08 00:21:05 $ + * $Date: 2004/10/07 19:07:05 $ */ Index: Events/EventRouter.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/EventRouter.cc,v retrieving revision 1.10 retrieving revision 1.13 diff -u -d -r1.10 -r1.13 --- Events/EventRouter.cc 3 Oct 2003 03:41:01 -0000 1.10 +++ Events/EventRouter.cc 7 Oct 2004 22:14:17 -0000 1.13 @@ -38,9 +38,14 @@ // cout << "done" << endl; } -/*! timers are unique by EventListener and source ID - can't have two timers for the same el and sid - * a delay of 0 with repeating will cause an event to be sent at every opportunity, use very sparingly - * a delay of -1U will call removeTimer() if it already exists, otherwise is ignored +/*! timers are unique by EventListener and source ID - can't have two timers for the same el and sid\n + * a delay of 0 with repeating will cause an event to be sent at every opportunity, use sparingly\n + * a delay of -1U will call removeTimer() if it already exists, otherwise is ignored\n + * + * To add a timer, you can also call addListener() with EventBase::timerEGID and the sid + * and delay (in the EventBase::duration field) - this method will simply cause this + * function to be called internally. + * * @param el the EventListener to send the timer event to * @param sid the source ID to use on that event (if you need to send more info, send a pointer to a struct of your devising, typecasted as int) * @param delay the delay between the first (and future) calls @@ -66,54 +71,220 @@ // chkTimers(); } -void EventRouter::addListener(EventListener* el, const EventBase& e) { - if(e.getGeneratorID()==EventBase::timerEGID) - addTimer(el,e.getSourceID(),e.getDuration()); - else - listeners.addMapping(el,e.getGeneratorID(),e.getSourceID(),e.getTypeID()); +void EventRouter::removeTimer(EventListener* el) { + for(timer_it_t it=timers.begin(); it!=timers.end(); it++) + if((*it)->el==el) { + delete *it; + *it=NULL; + } + timers.erase(remove(timers.begin(),timers.end(),(const TimerEntry*)NULL),timers.end()); +} + +void EventRouter::removeTimer(EventListener* el, unsigned int sid) { + for(timer_it_t it=timers.begin(); it!=timers.end(); it++) + if((*it)->el==el && (*it)->sid==sid) { + delete *it; + timers.erase(it); + return; + } +} + +void EventRouter::removeAllTimers() { + for(timer_it_t it=timers.begin(); it!=timers.end(); it++) + delete *it; + timers.erase(timers.begin(),timers.end()); } + void EventRouter::addListener(EventListener* el, EventBase::EventGeneratorID_t egid) { + bool hadListener=hasListeners(egid); listeners.addMapping(el,egid); + if(!hadListener) + postEvent(new EventBase(EventBase::erouterEGID,egid,EventBase::activateETID)); + else + postEvent(new EventBase(EventBase::erouterEGID,egid,EventBase::statusETID)); } void EventRouter::addListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid) { + bool hadListener=hasListeners(egid); for(unsigned int et=0; etel==el) { - delete *it; - *it=NULL; - } - timers.erase(remove(timers.begin(),timers.end(),(const TimerEntry*)NULL),timers.end()); +void EventRouter::forgetListener(EventListener* el) { + removeListener(el); } -void EventRouter::removeTimer(EventListener* el, unsigned int sid) { - for(timer_it_t it=timers.begin(); it!=timers.end(); it++) - if((*it)->el==el && (*it)->sid==sid) { - delete *it; - timers.erase(it); - return; - } +void EventRouter::addTrapper(EventTrapper* el, const EventBase& e) { + bool hadListener=hasListeners(e.getGeneratorID()); + trappers.addMapping(el,e.getGeneratorID(),e.getSourceID(),e.getTypeID()); + if(!hadListener) + postEvent(new EventBase(EventBase::erouterEGID,e.getGeneratorID(),EventBase::activateETID)); + else + postEvent(new EventBase(EventBase::erouterEGID,e.getGeneratorID(),EventBase::statusETID)); +} +/*! Note that since timers are not broadcast, they cannot be trapped. Only the EventListener which requested the timer will receive that timer. */ +void EventRouter::addTrapper(EventTrapper* el, EventBase::EventGeneratorID_t egid) { + bool hadListener=hasListeners(egid); + trappers.addMapping(el,egid); + if(!hadListener) + postEvent(new EventBase(EventBase::erouterEGID,egid,EventBase::activateETID)); + else + postEvent(new EventBase(EventBase::erouterEGID,egid,EventBase::statusETID)); +} +/*! Note that since timers are not broadcast, they cannot be trapped. Only the EventListener which requested the timer will receive that timer. */ +void EventRouter::addTrapper(EventTrapper* el, EventBase::EventGeneratorID_t egid, unsigned int sid) { + bool hadListener=hasListeners(egid); + for(unsigned int et=0; etpush_back(el); // now that everything's set up, we can add the listener } -void EventRouter::EventMapper::removeMapping(void* el, EventBase::EventGeneratorID_t egid) { +bool EventRouter::EventMapper::removeMapping(void* el, EventBase::EventGeneratorID_t egid) { // remove listener from allevents + unsigned int numlist=allevents[egid].size(); allevents[egid].erase(remove(allevents[egid].begin(),allevents[egid].end(),el),allevents[egid].end()); + bool hadListener=allevents[egid].size()!=numlist; // now remove listener from all of the filtered events for(unsigned int et=0; etbegin(); for(mapit=mapping->begin(); mapit!=mapping->end(); mapit++) {// go through each sourceID, delete EL std::vector * v=&(*mapit).second; - v->erase(remove(v->begin(),v->end(),el),v->end()); + std::vector::iterator last=remove(v->begin(),v->end(),el); + if(last!=v->end()) { + hadListener=true; + v->erase(last,v->end()); + } } } } + return hadListener; } -void EventRouter::EventMapper::removeMapping(void* el, EventBase::EventGeneratorID_t egid, unsigned int sid, EventBase::EventTypeID_t etid) { +bool EventRouter::EventMapper::removeMapping(void* el, EventBase::EventGeneratorID_t egid, unsigned int sid, EventBase::EventTypeID_t etid) { + bool hadListener=false; SIDtoListenerVectorMap_t* mapping=filteredevents[egid][etid]; if(mapping!=NULL) { // if there are subscribers to this egid/etid SIDtoListenerVectorMap_t::iterator mapit=mapping->find(sid); if(mapit!=mapping->end()) { std::vector * v=&(*mapit).second; - v->erase(remove(v->begin(),v->end(),el),v->end()); + std::vector::iterator last=remove(v->begin(),v->end(),el); + if(last!=v->end()) { + hadListener=true; + v->erase(last,v->end()); + } } } + return hadListener; } void EventRouter::EventMapper::clean() { + for(unsigned int eg=0; egbegin(); - bool done=false; - while(!done) { - done=true; - for(mapit=mapping->begin(); mapit!=mapping->end(); mapit++) { // go through each sourceID vector - if((*mapit).second.size()==0) { - mapping->erase(mapit); - done=false; - break; - } + for(unsigned int et=0; etbegin(); + bool done=false; + while(!done) { + done=true; + for(mapit=mapping->begin(); mapit!=mapping->end(); mapit++) { // go through each sourceID vector + if((*mapit).second.size()==0) { + mapping->erase(mapit); + done=false; + break; } } } } } // now remove any empty mappings - for(unsigned int eg=0; egsize()==0) { - delete mapping; - filteredevents[eg][et]=NULL; - } + for(unsigned int et=0; etsize()==0) { + delete mapping; + filteredevents[egid][et]=NULL; } } } @@ -349,9 +533,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ - * $State: Rel $ - * $Date: 2003/10/03 03:41:01 $ + * $Revision: 1.13 $ + * $State: Exp $ + * $Date: 2004/10/07 22:14:17 $ */ Index: Events/EventRouter.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/EventRouter.h,v retrieving revision 1.13 retrieving revision 1.16 diff -u -d -r1.13 -r1.16 --- Events/EventRouter.h 18 Jan 2004 10:16:57 -0000 1.13 +++ Events/EventRouter.h 7 Oct 2004 22:14:17 -0000 1.16 @@ -14,8 +14,13 @@ //! This class will handle distribution of events as well as management of timers /*! Classes must inherit from EventListener and/or EventTrapper in order to - * receive events.\n - * Use the global @c ::erouter EventRouter to post and subscribe to events\n + * receive events. + * + * Use the global @c ::erouter EventRouter to post and subscribe to + * events, except if you are posting from within a MotionCommand, in + * which case you should use MotionCommand::postEvent() so that it + * will automatically be sent from the Motion process to the Main + * process. * * When multiple listeners are subscribed, the order in which an event is * distributed among them looks like this: @@ -28,11 +33,32 @@ * ...but if you're relying on that ordering, there should be a cleaner * way to do whatever you're doing. * - * If one behaviors unsubscribes another one during a processEvent(), that - * behavior will still get the "current" event before the unsubscription - * takes place. + * If one behaviors unsubscribes another one during a processEvent(), + * that behavior may still get the "current" event before the + * unsubscription takes place. This is not a prescribed behavior, + * and also should not be relied on one way or the other. * - * Buffering events has not been tested thoroughly... + * Timer events are only sent to the generator which requested them. So if + * EventListener @e A requests a timer with ID 0 at two second intervals, + * and EventListener @e B requests a timer with ID 0 at three second intervals, + * each will still only receive the timers they requested - no cross talk. + * The timer generator is unique in this regard, which is why it is built in + * as an integral component of the EventRouter. All other events are broadcast. + * + * If an EventListener/EventTrapper subscribes to the same event source multiple + * times, it will receive multiple copies of the event. However, the first call + * to removeListener for a source will remove all subscriptions to that source.\n + * Example: EventListener @e A subscribes to (buttonEGID,*,*), and twice to + * (buttonEGID,0,*). + * - If button 0 is pressed, @e A will get three copies of the event. + * - If button 1 is pressed, @e A will get one copy. + * - If removeListener(&A,buttonEGID) is called, the (buttonEGID,*,*) is + * removed, as well as both of (buttonEGID,0,*). + * - If removeListener(&A,buttonEGID,0) is called, both of (buttonEGID,0,*) + * are removed, but (buttonEGID,*,*) would be untouched. + * + * The buffered event distribution has not been tested thoroughly, + * and should be considered deprecated. * * @see EventBase::EventGeneratorID_t for a complete listing of all generators, * as well as instructions on how to add new generators. @@ -67,10 +93,11 @@ bool hasListeners(EventBase::EventGeneratorID_t egid) { return trappers.hasMapping(egid) || listeners.hasMapping(egid); } bool hasListeners(EventBase::EventGeneratorID_t egid, unsigned int sid) { return trappers.hasMapping(egid,sid) || listeners.hasMapping(egid,sid); } bool hasListeners(EventBase::EventGeneratorID_t egid, unsigned int sid, EventBase::EventTypeID_t etid) { return trappers.hasMapping(egid,sid,etid) || listeners.hasMapping(egid,sid,etid); } + bool hasListeners(const EventBase& e) { return hasListeners(e.getGeneratorID(),e.getSourceID(),e.getTypeID()); } //@} //!@name Timer Management - void addTimer(EventListener* el, unsigned int sid, unsigned int delay, bool repeat=true); //!< adds a timer or sets a timer if it doesn't already exist. + void addTimer(EventListener* el, unsigned int sid, unsigned int delay, bool repeat=true); //!< adds a timer if it doesn't exist, or resets the timer if it already exists. void addTimer(EventListener* el, const EventBase& e, bool repeat=true) { addTimer(el,e.getSourceID(),e.getDuration(),repeat); } //!< calls the other addTimer() with the event's source id and duration, doesn't check to see if the generator is timerEGID void removeTimer(EventListener* el); //!< clears all pending timers for listener @a el void removeTimer(EventListener* el, unsigned int sid); //!< clears any pending timers with source id @a sid for listener @a el @@ -78,36 +105,44 @@ //@} //!@name Listener Management - void addListener(EventListener* el, const EventBase& e); //!< Adds a listener for a specific source id and type from a given event generator, adding a Timer event will invoke addTimer(@a el, @a e.getSourceID(), @a e.getDuration(), @c true) - void addListener(EventListener* el, EventBase::EventGeneratorID_t egid); //!< Adds a listener for all events from a given event generator + //! Adds a listener for all events from a given event generator + void addListener(EventListener* el, EventBase::EventGeneratorID_t egid); void addListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid); //!< Adds a listener for all types from a specific source and generator void addListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid, EventBase::EventTypeID_t etid); //!< Adds a listener for a specific source id and type from a given event generator + //! Adds a listener for a specific source id and type from a given event generator, adding a Timer event will invoke addTimer(@a el, @a e.getSourceID(), @a e.getDuration(), @c true) + void addListener(EventListener* el, const EventBase& e); + //! stops sending ALL events to the listener, including timers + void removeListener(EventListener* el); + //! stops sending specified events from the generator to the listener. + void removeListener(EventListener* el, EventBase::EventGeneratorID_t egid); + void removeListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid); //!< stops sending specified events from the generator to the listener. + void removeListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid, EventBase::EventTypeID_t etid); //!< stops sending specified events from the generator to the listener. //! stops sending specified events from the generator to the listener. If a timer is passed it will invoke removeTimer(@a el, @a e.getSourceID()) void removeListener(EventListener* el, const EventBase& e); - void removeListener(EventListener* el, EventBase::EventGeneratorID_t egid) { listeners.removeMapping(el,egid); listeners.clean(); } //!< stops sending specified events from the generator to the listener. - void removeListener(EventListener* el, EventBase::EventGeneratorID_t egid, unsigned int sid) { for(unsigned int et=0; etreserve(sizeof(TypeID_t)+len); if(buf==NULL) { - ASSERT(false,"Queue overflow"); + ASSERT(false,"Queue overflow "<(buf)=type; reinterpret_cast(buf)+=sizeof(TypeID_t); @@ -87,8 +87,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.6 $ + * $Revision: 1.7 $ * $State: Exp $ - * $Date: 2003/11/22 20:33:01 $ + * $Date: 2004/10/14 23:02:35 $ */ Index: Events/EventTranslator.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/EventTranslator.h,v retrieving revision 1.4 retrieving revision 1.5 diff -u -d -r1.4 -r1.5 --- Events/EventTranslator.h 22 Nov 2003 20:33:01 -0000 1.4 +++ Events/EventTranslator.h 16 Sep 2004 18:35:12 -0000 1.5 @@ -39,6 +39,9 @@ //!called by trapEvent to do all the work, needed so MotionCommands can enqueue directly static void enqueue(const EventBase& event, Queue_t * q); + //!called by non-MotionCommands to enqueue an event + void enqueue(const EventBase& event) { enqueue(event,queue); } + //!called whenever Main gets some processor time to check for events from other processes void translateEvents(); @@ -60,9 +63,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.4 $ + * $Revision: 1.5 $ * $State: Exp $ - * $Date: 2003/11/22 20:33:01 $ + * $Date: 2004/09/16 18:35:12 $ */ #endif Index: Events/LocomotionEvent.cc =================================================================== RCS file: Events/LocomotionEvent.cc diff -N Events/LocomotionEvent.cc --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Events/LocomotionEvent.cc 1 Sep 2004 21:30:57 -0000 1.1 @@ -0,0 +1,67 @@ +#include "LocomotionEvent.h" +#include + +std::string +LocomotionEvent::getDescription(bool showTypeSpecific/*=true*/, unsigned int verbosity/*=0*/) const { + if(!showTypeSpecific) + return EventBase::getDescription(showTypeSpecific,verbosity); + std::ostringstream logdata; + logdata << EventBase::getDescription(showTypeSpecific,verbosity) << '\t' << x << '\t' << y << '\t' << a; + return logdata.str(); +} + +unsigned int +LocomotionEvent::getBinSize() const { + unsigned int used=EventBase::getBinSize(); + used+=creatorSize("EventBase::LocomotionEvent"); + used+=sizeof(x); + used+=sizeof(y); + used+=sizeof(a); + return used; +} + +unsigned int +LocomotionEvent::LoadBuffer(const char buf[], unsigned int len) { + unsigned int origlen=len; + unsigned int used; + if(0==(used=EventBase::LoadBuffer(buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=checkCreator("EventBase::LocomotionEvent",buf,len,true))) return 0; + len-=used; buf+=used; + if(0==(used=decode(x,buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=decode(y,buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=decode(a,buf,len))) return 0; + len-=used; buf+=used; + return origlen-len; +} + +unsigned int +LocomotionEvent::SaveBuffer(char buf[], unsigned int len) const { + unsigned int origlen=len; + unsigned int used; + if(0==(used=EventBase::SaveBuffer(buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=saveCreator("EventBase::LocomotionEvent",buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=encode(x,buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=encode(y,buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=encode(a,buf,len))) return 0; + len-=used; buf+=used; + return origlen-len; +} + + +/*! @file + * @brief Implements LocomotionEvent, which gives updates regarding the current movement of the robot through the world + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2004/09/01 21:30:57 $ + */ Index: Events/LocomotionEvent.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/LocomotionEvent.h,v retrieving revision 1.5 retrieving revision 1.6 diff -u -d -r1.5 -r1.6 --- Events/LocomotionEvent.h 25 Sep 2003 15:27:10 -0000 1.5 +++ Events/LocomotionEvent.h 1 Sep 2004 21:30:57 -0000 1.6 @@ -28,47 +28,12 @@ a=A; return *this; } - - virtual unsigned int getBinSize() const { - unsigned int used=EventBase::getBinSize(); - used+=creatorSize("EventBase::LocomotionEvent"); - used+=sizeof(x); - used+=sizeof(y); - used+=sizeof(a); - return used; - } - virtual unsigned int LoadBuffer(const char buf[], unsigned int len) { - unsigned int origlen=len; - unsigned int used; - if(0==(used=EventBase::LoadBuffer(buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=checkCreator("EventBase::LocomotionEvent",buf,len,true))) return 0; - len-=used; buf+=used; - if(0==(used=decode(x,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=decode(y,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=decode(a,buf,len))) return 0; - len-=used; buf+=used; - return origlen-len; - } + virtual std::string getDescription(bool showTypeSpecific=true, unsigned int verbosity=0) const; - virtual unsigned int SaveBuffer(char buf[], unsigned int len) const { - unsigned int origlen=len; - unsigned int used; - if(0==(used=EventBase::SaveBuffer(buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=saveCreator("EventBase::LocomotionEvent",buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=encode(x,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=encode(y,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=encode(a,buf,len))) return 0; - len-=used; buf+=used; - return origlen-len; - } + virtual unsigned int getBinSize() const; + virtual unsigned int LoadBuffer(const char buf[], unsigned int len); + virtual unsigned int SaveBuffer(char buf[], unsigned int len) const; float x; //!< the new x component (body relative) float y; //!< the new y component (body relative) @@ -76,14 +41,14 @@ }; /*! @file - * @brief Defines LocomotionEvent, which gives updates regarding the current movement of the robot through the world + * @brief Describes LocomotionEvent, which gives updates regarding the current movement of the robot through the world * @author ejt (Creator) * * $Author: ejt $ * $Name: $ - * $Revision: 1.5 $ - * $State: Rel $ - * $Date: 2003/09/25 15:27:10 $ + * $Revision: 1.6 $ + * $State: Exp $ + * $Date: 2004/09/01 21:30:57 $ */ #endif Index: Events/TextMsgEvent.cc =================================================================== RCS file: Events/TextMsgEvent.cc diff -N Events/TextMsgEvent.cc --- /dev/null 1 Jan 1970 00:00:00 -0000 +++ Events/TextMsgEvent.cc 1 Sep 2004 21:30:57 -0000 1.1 @@ -0,0 +1,61 @@ +#include "TextMsgEvent.h" +#include + +std::string +TextMsgEvent::getDescription(bool showTypeSpecific/*=true*/, unsigned int verbosity/*=0*/) const { + if(!showTypeSpecific) + return EventBase::getDescription(showTypeSpecific,verbosity); + std::ostringstream logdata; + logdata << EventBase::getDescription(showTypeSpecific,verbosity) << '\t' << _text; + return logdata.str(); +} + +unsigned int +TextMsgEvent::getBinSize() const { + unsigned int used=EventBase::getBinSize(); + used+=creatorSize("EventBase::TextMsgEvent"); + used+=_text.size()+stringpad; + //used+=sizeof(_token); + return used; +} + +unsigned int +TextMsgEvent::LoadBuffer(const char buf[], unsigned int len) { + unsigned int origlen=len; + unsigned int used; + if(0==(used=EventBase::LoadBuffer(buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=checkCreator("EventBase::TextMsgEvent",buf,len,true))) return 0; + len-=used; buf+=used; + if(0==(used=decode(_text,buf,len))) return 0; + len-=used; buf+=used; + //if(0==(used=decode(_token,buf,len))) return 0; + //len-=used; buf+=used; + return origlen-len; +} + +unsigned int +TextMsgEvent::SaveBuffer(char buf[], unsigned int len) const { + unsigned int origlen=len; + unsigned int used; + if(0==(used=EventBase::SaveBuffer(buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=saveCreator("EventBase::TextMsgEvent",buf,len))) return 0; + len-=used; buf+=used; + if(0==(used=encode(_text,buf,len))) return 0; + len-=used; buf+=used; + //if(0==(used=encode(_token,buf,len))) return 0; + //len-=used; buf+=used; + return origlen-len; +} + +/*! @file + * @brief Implements TextMsgEvent, which extends EventBase to also include actual message text + * @author ejt (Creator) + * + * $Author: ejt $ + * $Name: $ + * $Revision: 1.1 $ + * $State: Exp $ + * $Date: 2004/09/01 21:30:57 $ + */ Index: Events/TextMsgEvent.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/TextMsgEvent.h,v retrieving revision 1.9 retrieving revision 1.10 diff -u -d -r1.9 -r1.10 --- Events/TextMsgEvent.h 5 Dec 2003 20:26:42 -0000 1.9 +++ Events/TextMsgEvent.h 1 Sep 2004 21:30:57 -0000 1.10 @@ -8,67 +8,37 @@ class TextMsgEvent : public EventBase { public: //! Constructor - TextMsgEvent() : EventBase(EventBase::textmsgEGID,(unsigned int)-1, EventBase::statusETID,0),_text(""),_token(0) { } + TextMsgEvent() : EventBase(EventBase::textmsgEGID,(unsigned int)-1, EventBase::statusETID,0),_text("")/*,_token(0)*/ { } //! Constructor, pass a text msg - TextMsgEvent(const std::string& text) : EventBase(EventBase::textmsgEGID,(unsigned int)-1, EventBase::statusETID,0),_text(text),_token(0) { } + TextMsgEvent(const std::string& text) : EventBase(EventBase::textmsgEGID,(unsigned int)-1, EventBase::statusETID,0),_text(text)/*,_token(0)*/ { } std::string getText() const { return _text; } //!< returns the text TextMsgEvent& setText(const std::string& text) { _text=text; return *this; } //!< sets the text - int getToken() const { return _token; } //!< returns the token - TextMsgEvent& setToken(int token) { _token=token; return *this;} //!< sets the token - - virtual unsigned int getBinSize() const { - unsigned int used=EventBase::getBinSize(); - used+=creatorSize("EventBase::TextMsgEvent"); - used+=_text.size()+stringpad; - used+=sizeof(_token); - return used; - } - - virtual unsigned int LoadBuffer(const char buf[], unsigned int len) { - unsigned int origlen=len; - unsigned int used; - if(0==(used=EventBase::LoadBuffer(buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=checkCreator("EventBase::TextMsgEvent",buf,len,true))) return 0; - len-=used; buf+=used; - if(0==(used=decode(_text,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=decode(_token,buf,len))) return 0; - len-=used; buf+=used; - return origlen-len; - } - - virtual unsigned int SaveBuffer(char buf[], unsigned int len) const { - unsigned int origlen=len; - unsigned int used; - if(0==(used=EventBase::SaveBuffer(buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=saveCreator("EventBase::TextMsgEvent",buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=encode(_text,buf,len))) return 0; - len-=used; buf+=used; - if(0==(used=encode(_token,buf,len))) return 0; - len-=used; buf+=used; - return origlen-len; - } + //int getToken() const { return _token; } //!< returns the token + //TextMsgEvent& setToken(int token) { _token=token; return *this;} //!< sets the token + + std::string getDescription(bool showTypeSpecific=true, unsigned int verbosity=0) const; + + virtual unsigned int getBinSize() const; + virtual unsigned int LoadBuffer(const char buf[], unsigned int len); + virtual unsigned int SaveBuffer(char buf[], unsigned int len) const; protected: std::string _text; //!< the unmodified arguments passed to the command - int _token; //!< for future expansion, to support centralized parsing + //int _token; //!< for future expansion, to support centralized parsing }; /*! @file - * @brief Defines TextMsgEvent, which extends EventBase to also include actual message text + * @brief Describes TextMsgEvent, which extends EventBase to also include actual message text * @author ejt (Creator) * * $Author: ejt $ * $Name: $ - * $Revision: 1.9 $ + * $Revision: 1.10 $ * $State: Exp $ - * $Date: 2003/12/05 20:26:42 $ + * $Date: 2004/09/01 21:30:57 $ */ #endif Index: Events/VisionObjectEvent.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/VisionObjectEvent.cc,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Events/VisionObjectEvent.cc 8 Dec 2003 00:21:05 -0000 1.2 +++ Events/VisionObjectEvent.cc 1 Sep 2004 21:30:57 -0000 1.3 @@ -1,4 +1,14 @@ #include "VisionObjectEvent.h" +#include + +std::string +VisionObjectEvent::getDescription(bool showTypeSpecific/*=true*/, unsigned int verbosity/*=0*/) const { + if(!showTypeSpecific) + return EventBase::getDescription(showTypeSpecific,verbosity); + std::ostringstream logdata; + logdata << EventBase::getDescription(showTypeSpecific,verbosity) << '\t' << _cenX << '\t' << _cenY; + return logdata.str(); +} unsigned int VisionObjectEvent::getBinSize() const { @@ -6,7 +16,7 @@ used+=creatorSize("EventBase::VisionObjectEvent"); used+=sizeof(_cenX); used+=sizeof(_cenY); - used+=sizeof(_distance); + //used+=sizeof(_distance); //used+=sizeof(_property); return used; } @@ -23,8 +33,8 @@ len-=used; buf+=used; if(0==(used=decode(_cenY,buf,len))) return 0; len-=used; buf+=used; - if(0==(used=decode(_distance,buf,len))) return 0; - len-=used; buf+=used; + //if(0==(used=decode(_distance,buf,len))) return 0; + //len-=used; buf+=used; //if(0==(used=decode(_property,buf,len))) return 0; //len-=used; buf+=used; return origlen-len; @@ -42,8 +52,8 @@ len-=used; buf+=used; if(0==(used=encode(_cenY,buf,len))) return 0; len-=used; buf+=used; - if(0==(used=encode(_distance,buf,len))) return 0; - len-=used; buf+=used; + //if(0==(used=encode(_distance,buf,len))) return 0; + //len-=used; buf+=used; //if(0==(used=encode(_property,buf,len))) return 0; //len-=used; buf+=used; return origlen-len; Index: Events/VisionObjectEvent.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Events/VisionObjectEvent.h,v retrieving revision 1.2 retrieving revision 1.3 diff -u -d -r1.2 -r1.3 --- Events/VisionObjectEvent.h 8 Dec 2003 00:21:05 -0000 1.2 +++ Events/VisionObjectEvent.h 1 Sep 2004 21:30:57 -0000 1.3 @@ -8,11 +8,11 @@ class VisionObjectEvent : public EventBase { public: //! Constructor - VisionObjectEvent() : EventBase(EventBase::visObjEGID,(unsigned int)-1,EventBase::statusETID,0),_cenX(0),_cenY(0),_distance()/*,_property(-1)*/ {} + VisionObjectEvent() : EventBase(EventBase::visObjEGID,(unsigned int)-1,EventBase::statusETID,0),_cenX(0),_cenY(0)/*,_distance(),_property(-1)*/ {} //! Constructor, pass a type id and source id - VisionObjectEvent(EventTypeID_t tid, unsigned int sid) : EventBase(EventBase::visObjEGID,sid,tid,0),_cenX(0),_cenY(0),_distance()/*,_property(-1)*/ {} + VisionObjectEvent(EventTypeID_t tid, unsigned int sid) : EventBase(EventBase::visObjEGID,sid,tid,0),_cenX(0),_cenY(0)/*_distance(),_property(-1)*/ {} //! Constructor, pass the type id, source id, center X and center Y - VisionObjectEvent(EventTypeID_t tid, unsigned int sid, float cenX, float cenY) : EventBase(EventBase::visObjEGID,sid,tid,0),_cenX(cenX),_cenY(cenY),_distance()/*,_property(-1)*/ {} + VisionObjectEvent(EventTypeID_t tid, unsigned int sid, float cenX, float cenY) : EventBase(EventBase::visObjEGID,sid,tid,0),_cenX(cenX),_cenY(cenY)/*,_distance(),_property(-1)*/ {} //! destructor virtual ~VisionObjectEvent() {} @@ -22,12 +22,14 @@ float getCenterY() const { return _cenY;} //!< returns the y coordinate VisionObjectEvent& setCenterY(float cenY) { _cenY=cenY; return *this;} //!< sets the y coordinate - float getDistance() const { return _distance;} //!< returns the distance (not implemented) - VisionObjectEvent& setDistance(float dist) { _distance=dist; return *this;} //!< sets the distance + //float getDistance() const { return _distance;} //!< returns the distance (not implemented) + //VisionObjectEvent& setDistance(float dist) { _distance=dist; return *this;} //!< sets the distance //int getProperty() const { return _property;} //!< returns the property (for future use) //VisionObjectEvent& setProperty(int property) { _property=property; return *this;} //!< sets the property + virtual std::string getDescription(bool showTypeSpecific=true, unsigned int verbosity=0) const; + virtual unsigned int getBinSize() const; virtual unsigned int LoadBuffer(const char buf[], unsigned int len); virtual unsigned int SaveBuffer(char buf[], unsigned int len) const; @@ -35,7 +37,7 @@ protected: float _cenX; //!< a value representing location in visual field - from -1 if on the left edge to 1 if it's on the right edge float _cenY; //!< a value representing location in visual field - from -1 if on the bottom edge to 1 if it's on the top edge - float _distance; //!< distance from snout to object in millimeters. (not implemented) + //float _distance; //!< distance from snout to object in millimeters. (not implemented) //int _property; //!< some property, depending on the SID (for future use) }; @@ -45,9 +47,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.2 $ + * $Revision: 1.3 $ * $State: Exp $ - * $Date: 2003/12/08 00:21:05 $ + * $Date: 2004/09/01 21:30:57 $ */ #endif Index: MMCombo/MMCombo.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/MMCombo/MMCombo.cc,v retrieving revision 1.50 retrieving revision 1.57 diff -u -d -r1.50 -r1.57 --- MMCombo/MMCombo.cc 3 Mar 2004 03:37:03 -0000 1.50 +++ MMCombo/MMCombo.cc 11 Oct 2004 22:00:19 -0000 1.57 @@ -8,6 +8,7 @@ #include "Events/EventRouter.h" #include "Behaviors/BehaviorBase.h" #include "Motion/MotionManager.h" +#include "Motion/Kinematics.h" #include "SoundPlay/SoundManager.h" #include "Events/DataEvent.h" #include "Events/TextMsgEvent.h" @@ -53,9 +54,28 @@ // make sure the library doesn't drop data "for" us on this reliable communication channel observer[obsReceiveWorldState]->SetBufCtrlParam(0,1,1); observer[obsReceiveMotionManager]->SetBufCtrlParam(0,1,1); + observer[obsReceiveSoundManager]->SetBufCtrlParam(0,1,1); observer[obsMotionManagerComm]->SetBufCtrlParam(0,1,MotionManager::MAX_MOTIONS+1); //+1 to MAX_MOTIONS so we can get a delete message after we've filled up + cout << objectName << ": sbjRegisterWorldState==" << sbjRegisterWorldState << " selector==" << subject[sbjRegisterWorldState]->GetID().GetSelector() << '\n' + << objectName << ": obsReceiveWorldState==" << obsReceiveWorldState << " selector==" << observer[obsReceiveWorldState]->GetID().GetSelector() << '\n' + << objectName << ": sbjRegisterMotionManager==" << sbjRegisterMotionManager << " selector==" << subject[sbjRegisterMotionManager]->GetID().GetSelector() << '\n' + << objectName << ": obsReceiveMotionManager==" << obsReceiveMotionManager << " selector==" << observer[obsReceiveMotionManager]->GetID().GetSelector() << '\n' + << objectName << ": sbjRegisterEventTranslatorQueue==" << sbjRegisterEventTranslatorQueue << " selector==" << subject[sbjRegisterEventTranslatorQueue]->GetID().GetSelector() << '\n' + << objectName << ": obsReceiveEventTranslatorQueue==" << obsReceiveEventTranslatorQueue << " selector==" << observer[obsReceiveEventTranslatorQueue]->GetID().GetSelector() << '\n' + << objectName << ": sbjMoveJoint==" << sbjMoveJoint << " selector==" << subject[sbjMoveJoint]->GetID().GetSelector() << '\n' + << objectName << ": obsSensorFrame==" << obsSensorFrame << " selector==" << observer[obsSensorFrame]->GetID().GetSelector() << '\n' + << objectName << ": obsImage==" << obsImage << " selector==" << observer[obsImage]->GetID().GetSelector() << '\n' + << objectName << ": obsMic==" << obsMic << " selector==" << observer[obsMic]->GetID().GetSelector() << '\n' + << objectName << ": sbjMotionManagerComm==" << sbjMotionManagerComm << " selector==" << subject[sbjMotionManagerComm]->GetID().GetSelector() << '\n' + << objectName << ": obsMotionManagerComm==" << obsMotionManagerComm << " selector==" << observer[obsMotionManagerComm]->GetID().GetSelector() << '\n' + << objectName << ": obsReceiveSoundManager==" << obsReceiveSoundManager << " selector==" << observer[obsReceiveSoundManager]->GetID().GetSelector() << '\n' + << objectName << ": sbjSoundManagerComm==" << sbjSoundManagerComm << " selector==" << subject[sbjSoundManagerComm]->GetID().GetSelector() << '\n' + << objectName << ": sbjRPOPENRSendString==" << sbjRPOPENRSendString << " selector==" << subject[sbjRPOPENRSendString]->GetID().GetSelector() << '\n' + << objectName << ": obsRPOPENRReceiveString==" << obsRPOPENRReceiveString << " selector==" << observer[obsRPOPENRReceiveString]->GetID().GetSelector() << '\n' + << flush; + if(strcmp(objectName,"MainObj")==0) ProcessID::setID(ProcessID::MainProcess); else if(strcmp(objectName,"MotoObj")==0) @@ -82,7 +102,7 @@ OSYSLOG1((osyslogERROR, "%s : %s %d","MMCombo::DoStart()","OPENR::ObservePowerStatus() FAILED", result)); return oFAIL; } - + //Setup wireless wireless = new Wireless(); sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6); @@ -109,11 +129,23 @@ OPENR::SetMotorPower(opowerON); OPENR::EnableJointGain(oprimitiveID_UNDEF); //oprimitiveID_UNDEF means enable all + //Setup wireless + wireless = new Wireless(); + sout=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*6); + serr=wireless->socket(SocketNS::SOCK_STREAM,Wireless::WIRELESS_DEF_RECV_SIZE,Wireless::WIRELESS_DEF_SEND_SIZE*2); + wireless->setDaemon(sout); + wireless->setDaemon(serr); + serr->setFlushType(SocketNS::FLUSH_BLOCKING); + sout->setTextForward(); + serr->setForward(sout); + //motmanMemRgn -> motman setup motmanMemRgn = InitRegion(sizeof(MotionManager)); motman = new (motmanMemRgn->Base()) MotionManager(); motman->InitAccess(subject[sbjMotionManagerComm]); } + kine = new Kinematics(); + ProjectInterface::startupBehavior.AddReference(); cout << objectName << "::DoInit()-DONE" << endl; @@ -135,6 +167,11 @@ state->read(power,erouter); } + if(strcmp(objectName,"MotoObj")==0) { + wireless->listen(sout, config->motion.console_port); + wireless->listen(serr, config->motion.stderr_port); + } + isStopped=false; ENABLE_ALL_SUBJECT; @@ -475,6 +512,8 @@ PROFSECTION("GotImage()",state->mainProfile); etrans.translateEvents(); + erouter->processTimers(); + WMvari(int, frame_counter, 0); ++frame_counter; @@ -486,6 +525,23 @@ } void +MMCombo::GotAudio(const ONotifyEvent& event){ + if(isStopped) { + //cout << "BAH!GotAudio" << endl; + return; + } + + PROFSECTION("GotAudio()",state->mainProfile); + etrans.translateEvents(); + + erouter->postEvent(new DataEvent(reinterpret_cast(event.Data(0)),EventBase::micOSndEGID,0,EventBase::statusETID)); + + erouter->processTimers(); + + observer[obsMic]->AssertReady(); +} + +void MMCombo::GotPowerEvent(void * msg){ if(isStopped) { //cout << "BAH!GotPowerEvent" << endl; @@ -712,9 +768,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.50 $ + * $Revision: 1.57 $ * $State: Exp $ - * $Date: 2004/03/03 03:37:03 $ + * $Date: 2004/10/11 22:00:19 $ */ Index: MMCombo/MMCombo.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/MMCombo/MMCombo.h,v retrieving revision 1.25 retrieving revision 1.26 diff -u -d -r1.25 -r1.26 --- MMCombo/MMCombo.h 3 Feb 2004 01:17:24 -0000 1.25 +++ MMCombo/MMCombo.h 28 Sep 2004 23:07:04 -0000 1.26 @@ -51,6 +51,9 @@ void ReadySendJoints(const OReadyEvent& event); //!< motion only (until main does ears again, then both) calls SendJoints, if DoStart has already been called void GotSensorFrame(const ONotifyEvent& event); //!< main only, called when new sensor information is available void GotImage(const ONotifyEvent& event); //!< main only, called when a new image is available +// paris start + void GotAudio(const ONotifyEvent& event); //!< main only, called when a new audio buffer is available +// paris end void GotPowerEvent(void * msg); //!< main only, called when a power event occurs (can be just status events) void GotMotionMsg(const ONotifyEvent& event); //!< both, called when a new MotionManagerMsg has been received @@ -139,9 +142,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.25 $ + * $Revision: 1.26 $ * $State: Exp $ - * $Date: 2004/02/03 01:17:24 $ + * $Date: 2004/09/28 23:07:04 $ */ #endif Index: MMCombo/stub.cfg =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/MMCombo/stub.cfg,v retrieving revision 1.6 retrieving revision 1.7 diff -u -d -r1.6 -r1.7 --- MMCombo/stub.cfg 6 Apr 2003 20:57:44 -0000 1.6 +++ MMCombo/stub.cfg 28 Sep 2004 23:07:04 -0000 1.7 @@ -1,6 +1,6 @@ ObjectName : MMCombo NumOfSubject : 7 -NumOfObserver : 8 +NumOfObserver : 9 Service : "MMCombo.RegisterWorldState.WorldState.S" , null, ReadyRegisterWorldState() Service : "MMCombo.ReceiveWorldState.WorldState.O" , null, GotWorldState() Service : "MMCombo.RegisterMotionManager.MotionManager.S" , null, ReadyRegisterMotionManager() @@ -10,6 +10,7 @@ Service : "MMCombo.MoveJoint.OCommandVectorData.S" , null, ReadySendJoints() Service : "MMCombo.SensorFrame.OSensorFrameVectorData.O" , null, GotSensorFrame() Service : "MMCombo.Image.OFbkImageVectorData.O" , null, GotImage() +Service : "MMCombo.Mic.OSoundVectorData.O" , null, GotAudio() Service : "MMCombo.MotionManagerComm.MotionManagerMsg.S" , null, null Service : "MMCombo.MotionManagerComm.MotionManagerMsg.O" , null, GotMotionMsg() Service : "MMCombo.ReceiveSoundManager.SoundManager.O" , null, GotSoundManager() Index: Motion/EmergencyStopMC.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/EmergencyStopMC.cc,v retrieving revision 1.20 retrieving revision 1.21 diff -u -d -r1.20 -r1.21 --- Motion/EmergencyStopMC.cc 6 Feb 2004 23:13:46 -0000 1.20 +++ Motion/EmergencyStopMC.cc 30 Aug 2004 20:26:45 -0000 1.21 @@ -105,6 +105,12 @@ dirty=true; } +void EmergencyStopMC::takeSnapshot(const WorldState* st) { + for(unsigned int i=0; ioutputs[i]; + dirty=true; +} + void EmergencyStopMC::setActive(bool a) { if(paused) { if(!a && active) @@ -204,8 +210,8 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.20 $ + * $Revision: 1.21 $ * $State: Exp $ - * $Date: 2004/02/06 23:13:46 $ + * $Date: 2004/08/30 20:26:45 $ */ Index: Motion/EmergencyStopMC.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/EmergencyStopMC.h,v retrieving revision 1.10 retrieving revision 1.11 diff -u -d -r1.10 -r1.11 --- Motion/EmergencyStopMC.h 18 Jan 2004 10:16:57 -0000 1.10 +++ Motion/EmergencyStopMC.h 30 Aug 2004 20:26:45 -0000 1.11 @@ -28,6 +28,7 @@ virtual int updateOutputs(); //!< checks for feedback or double tap virtual void takeSnapshot(); //!< records current positions of joints + virtual void takeSnapshot(const WorldState* st); //!< records current positions of joints void setActive(bool a); //!< allows you to modify #active bool getActive() { return active; } //!< returns #active @@ -62,9 +63,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.10 $ + * $Revision: 1.11 $ * $State: Exp $ - * $Date: 2004/01/18 10:16:57 $ + * $Date: 2004/08/30 20:26:45 $ */ #endif Index: Motion/HeadPointerMC.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/HeadPointerMC.cc,v retrieving revision 1.9 retrieving revision 1.14 diff -u -d -r1.9 -r1.14 --- Motion/HeadPointerMC.cc 19 Jan 2004 07:56:16 -0000 1.9 +++ Motion/HeadPointerMC.cc 14 Oct 2004 21:59:23 -0000 1.14 @@ -5,28 +5,14 @@ #include #include "Shared/Config.h" -HeadPointerMC::HeadPointerMC() : - MotionCommand(), dirty(true), active(true) { +HeadPointerMC::HeadPointerMC() + : MotionCommand(), dirty(true), targetReached(false), + headkin(::config->motion.makePath(::config->motion.kinematics),"Camera") +{ setWeight(1); defaultMaxSpeed(); - for(unsigned int i=0; ioutputs[HeadOffset+i]; - } -} - -void HeadPointerMC::setJoints(double tilt, double pan, double roll) { - headValues[TiltOffset]=tilt; - headValues[PanOffset]=pan; - headValues[RollOffset]=roll; - dirty=true; -} - -void HeadPointerMC::setWeight(double w) { - for(unsigned int x=0; xoutputs[HeadOffset+i]; } void HeadPointerMC::defaultMaxSpeed() { @@ -35,16 +21,50 @@ maxSpeed[RollOffset]=config->motion.max_head_roll_speed*FrameTime/1000; } -void HeadPointerMC::setMode(CoordFrame_t m, bool conv) { +void HeadPointerMC::setWeight(float w) { for(unsigned int x=0; xsetOutput(this,i+HeadOffset,headJoints[i]); + if(maxSpeed[i]<=0) { + headCmds[i].value=headTargets[i]; + motman->setOutput(this,i+HeadOffset,headCmds[i]); } else { // we may be trying to exceeded maxSpeed unsigned int f=0; - while(value>headJoints[i].value+maxSpeed[i] && fsetOutput(this,i+HeadOffset,headJoints[i],f); + while(headTargets[i]>headCmds[i].value+maxSpeed[i] && fsetOutput(this,i+HeadOffset,headCmds[i],f); f++; } - while(valuesetOutput(this,i+HeadOffset,headJoints[i],f); + while(headTargets[i]setOutput(this,i+HeadOffset,headCmds[i],f); f++; } if(fsetOutput(this,i+HeadOffset,headJoints[i],f); + motman->setOutput(this,i+HeadOffset,headCmds[i],f); } else // we didn't reach target value, still dirty dirty=true; } } - } - return tmp; -} - -const OutputCmd& HeadPointerMC::getOutputCmd(unsigned int i) { - i-=HeadOffset; - if(isensors[BAccelOffset]; - double lacc=state->sensors[LAccelOffset]; - double dacc=state->sensors[DAccelOffset]; - switch(i) { - case TiltOffset: - return v+atan2(bacc,sqrt(dacc*dacc+lacc*lacc)); - case PanOffset: - return v; - case RollOffset: //==NodOffset - if(state->robotDesign&WorldState::ERS7Mask) { - float pans=sin(state->outputs[HeadOffset+PanOffset]); - float panc=cos(state->outputs[HeadOffset+PanOffset]); - float ans=v; - ans+=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans)); - ans-=panc*state->outputs[HeadOffset+TiltOffset]; - return ans; - } else - return v+atan2(-lacc,sqrt(dacc*dacc+bacc*bacc)); - default: - ASSERT(false,"Passed bad offset "<sensors[BAccelOffset]; - double lacc=state->sensors[LAccelOffset]; - double dacc=state->sensors[DAccelOffset]; - switch(i) { - case TiltOffset: - return v-atan2(bacc,sqrt(dacc*dacc+lacc*lacc)); - case PanOffset: - return v; - case RollOffset: //==NodOffset - if(state->robotDesign&WorldState::ERS7Mask) { - float pans=sin(state->outputs[HeadOffset+PanOffset]); - float panc=cos(state->outputs[HeadOffset+PanOffset]); - float ans=v; - ans-=atan2(bacc*panc-lacc*pans,sqrt(dacc*dacc+lacc*lacc*panc*panc+bacc*bacc*pans*pans)); - ans+=panc*state->outputs[HeadOffset+TiltOffset]; - return ans; - } else - return v-atan2(-lacc,sqrt(dacc*dacc+bacc*bacc)); - default: - ASSERT(false,"Passed bad offset "<max) { + float mn_dist=fabs(normalizeAngle(min-x)); + float mx_dist=fabs(normalizeAngle(max-x)); + if(mn_dist -#include -#endif - -#include - #include "Kinematics.h" +#include "Shared/Config.h" +#include "roboop/robot.h" +#include "roboop/config.h" +#include "Shared/WorldState.h" +#include -// #define DEBUG - -// ERS-210 Leg Parameters -const vector3d f_body_to_shoulder ( 59.50, 59.20, 0.00); -const vector3d f_leg_shoulder_to_knee ( 12.80, 0.50, -64.00); -const vector3d f_leg_knee_to_ball (-18.00, 0.00, -67.23); - -const vector3d h_body_to_shoulder (-59.50, 59.20, 0.00); -const vector3d h_leg_shoulder_to_knee (-12.80, 0.50, -64.00); -const vector3d h_leg_knee_to_ball ( 18.00, 0.00, -74.87); +using namespace std; -/* - 18 : measured - 67.23 = sqrt(69.6^2 - 18^2) - 74.87 = sqrt(77^2 - 18^2) - 0.2616 = asin(18 / 69.6) - 0.2316 = asin(18 / 77) -*/ +Kinematics * kine = NULL; +ROBOOP::Config * Kinematics::roconfig = NULL; +Kinematics::InterestPointMap * Kinematics::ips = NULL; -/* ERS-110 -const vector3d f_body_to_shoulder ( 44.85, 26.50, 0.00); -const vector3d f_leg_shoulder_to_knee ( 13.00, 5.50, -61.00); -const vector3d f_leg_knee_to_ball ( -9.50, 1.06, -58.00); // ?? +void +Kinematics::init() { + unsigned int nchains=::config->motion.kinematic_chains.size(); + if(nchains==0) { + serr->printf("ERROR Kinematics::init(): no kinematic chains were selected\n"); + return; + } + jointMaps[BaseFrameOffset]=JointMap(0,0); + chains.resize(nchains); + chainMaps.resize(nchains); + if(roconfig==NULL) + roconfig=new ROBOOP::Config(::config->motion.makePath(::config->motion.kinematics),true); + for(unsigned int i=0; imotion.kinematic_chains[i]; + chains[i]=new ROBOOP::Robot(*roconfig,section); + int dof=0; + if(roconfig->select_int(section,"dof",dof)!=0) { + serr->printf("ERROR Kinematics::init(): unable to find 'dof' in chain %d (%s)\n",i,section.c_str()); + return; + } + chainMaps[i].resize(dof+1); + for(int l=0; l<=dof; l++) + chainMaps[i][l]=-1U; + for(int l=1; l<=dof; l++) { + ostringstream ostr; + ostr << section << "_LINK" << l; + string link = ostr.str(); + if(roconfig->parameter_exists(link,"tekkotsu_output")) { + int tkout=-1U; + roconfig->select_int(link,"tekkotsu_output",tkout); + if((unsigned int)tkout>=NumOutputs) { + serr->printf("WARNING Kinematics::init(): invalid tekkotsu_output %d on chain %d (%s), link %d (%s)\n",tkout,i,section.c_str(),l,link.c_str()); + } else { + jointMaps[tkout]=JointMap(i,l); + chainMaps[i][l]=tkout; + } + } + if(roconfig->parameter_exists(link,"tekkotsu_frame")) { + int tkout=-1U; + roconfig->select_int(link,"tekkotsu_frame",tkout); + tkout+=NumOutputs; + if((unsigned int)tkout>=NumReferenceFrames) + serr->printf("WARNING Kinematics::init(): invalid tekkotsu_frame %d on chain %d (%s), link %d (%s)\n",tkout,i,section.c_str(),l,link.c_str()); + else + jointMaps[tkout]=JointMap(i,l); + } + } + } -const vector3d h_body_to_shoulder (-44.85, 26.50, 0.00); -const vector3d h_leg_shoulder_to_knee (-13.00, 5.50, -61.00); -const vector3d h_leg_knee_to_ball ( 19.00, 1.06, -69.00); // ?? -*/ + if(ips==NULL) { + int numIP=0; + roconfig->select_int("InterestPoints","Length",numIP); + ips=new InterestPointMap(numIP); + pair ip; + string chain; + for(int i=1; i<=numIP; i++) { + char section[100]; + snprintf(section,100,"InterestPoint%d",i); + roconfig->select_float(section,"x",ip.second.x); + roconfig->select_float(section,"y",ip.second.y); + roconfig->select_float(section,"z",ip.second.z); + roconfig->select_string(section,"chain",chain); + roconfig->select_int(section,"link",ip.second.link); + roconfig->select_string(section,"name",ip.first); + for(ip.second.chain=0; ip.second.chain<(::config->motion.kinematic_chains.size()); ip.second.chain++) + if(::config->motion.kinematic_chains[ip.second.chain]==chain) + break; + if(ip.second.chain==::config->motion.kinematic_chains.size()) + serr->printf("WARNING: Chain %s is not recognized for interest point %d\n",chain.c_str(),i); + else + ips->insert(ip); + } + } -#ifdef PLATFORM_LINUX -void print(double *angles,vector3d pos) -{ - printf("A(%7.4f,%7.4f,%7.4f) P(%7.2f,%7.2f,%7.2f)\n", - angles[0],angles[1],angles[2], - pos.x,pos.y,pos.z); + /*cout << "Joint Maps" << endl; + for(unsigned int i=0; iconvertFrame(l,0); } -int KinGetErrors() -{ - return(errors); +NEWMAT::ReturnMatrix +Kinematics::linkToBase(unsigned int j) { + unsigned int c=-1U,l=-1U; + if(!lookup(j,c,l)) { + NEWMAT::Matrix A(4,4); + A<convertLink(l,0); } -double GetTrigAngle(double a,double b,double d,double mn,double mx,bool add) -// Analytic solution to a*sin(x) + b*cos(x) = d -{ - double theta; - double t,f,c; - int err; - - f = d / sqrt(a*a + b*b); - err = 0; - - if(fabs(f) > 1.0){ -#ifdef PLATFORM_LINUX - /* - printf("Out of range (distance=%g) leg=%d target=(%g,%g,%g)\n",f, - g_leg,g_target.x,g_target.y,g_target.z); - */ -#endif - f = (f > 0.0)? 1.0 : -1.0; - err = 1; - } - - t = atan2(a,b); - c = acos(f); - - theta = add? (t + c) : (t - c); - - if(theta < mn){ -#ifdef PLATFORM_LINUX - /* - printf("Out of range (angle to small) leg=%d target=(%g,%g,%g)\n", - g_leg,g_target.x,g_target.y,g_target.z); - */ -#endif - errors++; - return(mn); - }else if(theta > mx){ -#ifdef PLATFORM_LINUX - /* - printf("Out of range (angle to large) leg=%d target=(%g,%g,%g)\n", - g_leg,g_target.x,g_target.y,g_target.z); - */ -#endif - errors++; - return(mx); - }else{ - errors += err; - return(theta); - } +NEWMAT::ReturnMatrix +Kinematics::baseToFrame(unsigned int j) { + unsigned int c=-1U,l=-1U; + if(!lookup(j,c,l)) { + NEWMAT::Matrix A(4,4); + A<convertFrame(0,l); } -/* -Angle Limits: - ===Software==== ==Mechanical=== - Rotator [-117.0, 117.0] [-120.0, 120.0] - Shoulder [ -11.0, 97.0] [ -14.0, 100.0] - Knee [ -27.0, 147.0] [ -30.0, 150.0] -*/ - -const double rotator_min = RAD(-117.0); -const double rotator_max = RAD( 117.0); -const double shoulder_min = RAD( -11.0); -const double shoulder_max = RAD( 97.0); -const double knee_max = RAD( 147.0); -const double knee_min = RAD( -27.0); - -const double rotator_kmin = RAD(-90.0); -const double rotator_kmax = RAD( 90.0); -const double shoulder_kmin = shoulder_min; -const double shoulder_kmax = RAD( 90.0); -const double knee_kmax = knee_max; -const double f_knee_kmin = 0.2616; -const double h_knee_kmin = 0.2316; - -const double tail_min = RAD(-22); -const double tail_max = RAD( 22); - -const double head_tilt_min = RAD(-88.5); -const double head_tilt_max = RAD( 43.0); -const double head_pan_min = RAD(-89.6); -const double head_pan_max = RAD( 89.6); -const double head_roll_min = RAD(-29.0); -const double head_roll_max = RAD( 29.0); - - -void GetLegAngles(double *angles,vector3d target,int leg) -{ - vector3d targ,pos; - double knee,shoulder,rotator; - double a,b,d,dist; - bool front = leg < 2; - -#ifdef PLATFORM_LINUX - g_leg = leg; - g_target = target; - // printf("GLA: target=(%g,%g,%g)\n",target.x,target.y,target.z); -#endif - - knee = shoulder = rotator = 0.0; - - if(leg % 2) target.y = -target.y; - - if(front){ - targ = target - f_body_to_shoulder; - dist = targ.sqlength(); - - // Calculate knee angle - a = -2*(f_upper.x*f_lower.z - f_upper.z*f_lower.x); - b = 2*(f_upper.x*f_lower.x + f_upper.z*f_lower.z); - d = (dist - f_upper.sqlength() - f_lower.sqlength() - 2*f_upper.y*f_lower.y); - knee = GetTrigAngle(a,b,d,f_knee_kmin,knee_kmax,true); - - // Calculate shoulder angle - pos = f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-knee); - shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false); - - // Calculate rotator angle - // pos = pos.rotate_x(shoulder); - pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z; - rotator = GetTrigAngle(-pos.z,pos.x,targ.x,rotator_min,rotator_max,target.z > 0.0); - -#ifdef DEBUG - // Test - // pos = (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-knee)) - // .rotate_x(shoulder).rotate_y(-rotator); - // printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z); -#endif - }else{ - targ = target - h_body_to_shoulder; - dist = targ.sqlength(); - - // Calculate knee angle - a = 2*(h_upper.x*h_lower.z - h_upper.z*h_lower.x); - b = 2*(h_upper.x*h_lower.x + h_upper.z*h_lower.z); - d = (dist - h_upper.sqlength() - h_lower.sqlength() - 2*h_upper.y*h_lower.y); - knee = GetTrigAngle(a,b,d,h_knee_kmin,knee_kmax,true); - - // Calculate shoulder angle - pos = h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y(knee); - shoulder = GetTrigAngle(-pos.z,pos.y,targ.y,shoulder_kmin,shoulder_kmax,false); - - // Calculate rotator angle - pos.z = sin(shoulder)*pos.y + cos(shoulder)*pos.z; - rotator = GetTrigAngle(-pos.z,-pos.x,-targ.x,rotator_min,rotator_max,target.z > 0.0); - - /* - if(fabs(theta - rotator) > 0.01){ - printf("ERROR(%f - %f = %f)\n",theta,rotator,theta - rotator); - } - */ - -#ifdef DEBUG - // Test - // pos = (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y(angles[2])) - // .rotate_x(angles[1]).rotate_y(angles[0]); - // printf("D(%f,%f,%f)\n",targ.x - pos.x,targ.y - pos.y,targ.z - pos.z); -#endif - } - - angles[0] = rotator; - angles[1] = shoulder; - angles[2] = knee; +NEWMAT::ReturnMatrix +Kinematics::baseToLink(unsigned int j) { + unsigned int c=-1U,l=-1U; + if(!lookup(j,c,l)) { + NEWMAT::Matrix A(4,4); + A<convertLink(0,l); } -void GetLegAngles(double *angles,vector3d target[4], - double body_angle,double body_height) -{ - vector3d p; - int i; +NEWMAT::ReturnMatrix +Kinematics::frameToFrame(unsigned int ij, unsigned int oj) { + unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U; + if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) { + NEWMAT::Matrix A(4,4); + A<lo?li:lo); + return chains[ci]->convertFrame(li,lo); + } else if(li==0) { + update(co,lo); + return chains[co]->convertFrame(0,lo); + } else if(lo==0) { + update(ci,li); + return chains[ci]->convertFrame(li,0); + } else { + update(ci,li); + update(co,lo); + NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertFrame(li,0); + ans.Release(); return ans; + } +} - for(i=0; i<4; i++){ - p = target[i]; - p.z -= body_height; - p = p.rotate_y(-body_angle); - GetLegAngles(angles + 3*i,p,i); - } +NEWMAT::ReturnMatrix +Kinematics::frameToLink(unsigned int ij, unsigned int oj) { + unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U; + if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) { + NEWMAT::Matrix A(4,4); + A<lo?li:lo); + return chains[ci]->convertFrameToLink(li,lo); + } else if(li==0) { + update(co,lo); + return chains[co]->convertFrameToLink(0,lo); + } else if(lo==0) { + update(ci,li); + return chains[ci]->convertFrameToLink(li,0); + } else { + update(ci,li); + update(co,lo); + NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertFrame(li,0); + ans.Release(); return ans; + } } -void GetLegAngles(double *angles,vector3d target[4],BodyPosition &bp) -{ - vector3d p; - int i; +NEWMAT::ReturnMatrix +Kinematics::linkToFrame(unsigned int ij, unsigned int oj) { + unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U; + if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) { + NEWMAT::Matrix A(4,4); + A<lo?li:lo); + return chains[ci]->convertLinkToFrame(li,lo); + } else if(li==0) { + update(co,lo); + return chains[co]->convertLinkToFrame(0,lo); + } else if(lo==0) { + update(ci,li); + return chains[ci]->convertLinkToFrame(li,0); + } else { + update(ci,li); + update(co,lo); + NEWMAT::Matrix ans=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0); + ans.Release(); return ans; + } +} - for(i=0; i<4; i++){ - p = (target[i] - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y); - GetLegAngles(angles + 3*i,p,i); - } +NEWMAT::ReturnMatrix +Kinematics::linkToLink(unsigned int ij, unsigned int oj) { + unsigned int ci=-1U,li=-1U,co=-1U,lo=-1U; + if(ij==oj || !lookup(ij,ci,li) || !lookup(oj,co,lo)) { + NEWMAT::Matrix A(4,4); + A<lo?li:lo); + return chains[ci]->convertLink(li,lo); + } else if(li==0) { + update(co,lo); + return chains[co]->convertLink(0,lo); + } else if(lo==0) { + update(ci,li); + return chains[ci]->convertLink(li,0); + } else { + update(ci,li); + update(co,lo); + NEWMAT::Matrix ans=chains[co]->convertLink(0,lo)*chains[ci]->convertLink(li,0); + ans.Release(); return ans; + } } -void GetLegAngles(double *angles,vector3d target,BodyPosition &bp,int leg) -{ - vector3d p; +void +Kinematics::getInterestPoint(const std::string& name, unsigned int& j, NEWMAT::Matrix& ip) { + unsigned int c=-1U,l=-1U; + getInterestPoint(name,c,l,ip); + j=chainMaps[c][l]; +} - p = (target - bp.loc).rotate_z(bp.angle.z).rotate_y(-bp.angle.y); - GetLegAngles(angles,p,leg); +void +Kinematics::getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip) { + InterestPointMap::iterator it=ips->find(name); + ip=NEWMAT::ColumnVector(4); + if(it==ips->end()) { + serr->printf("ERROR: '%s' is not a recognized interest point\n",name.c_str()); + ip=0; + c=l=-1U; + } + c=(*it).second.chain; + l=(*it).second.link; + ip=pack((*it).second.x,(*it).second.y,(*it).second.z); + //cout << ci << ' ' << li << ' ' << co << ' ' << lo << endl; } +NEWMAT::ReturnMatrix +Kinematics::getFrameInterestPoint(unsigned int frame, const std::string& name) { + NEWMAT::ColumnVector ans(4); + ans=0; + unsigned int co=-1U,lo=-1U; + if(!lookup(frame,co,lo)) { + ans.Release(); return ans; + } + for(unsigned int pos=0,len=0; poslo?li:lo); + ans+=chains[ci]->convertLinkToFrame(li,lo)*ip; + } else if(li==0) { + update(co,lo); + ans+=chains[co]->convertLinkToFrame(0,lo)*ip; + } else if(lo==0) { + update(ci,li); + ans+=chains[ci]->convertLinkToFrame(li,0)*ip; + } else { + update(ci,li); + update(co,lo); + ans+=chains[co]->convertFrame(0,lo)*chains[ci]->convertLink(li,0)*ip; + } + } + ans/=ans(4); //not strictly necessary, but may save some people headaches + ans.Release(); return ans; +} -void GetLegPosition(vector3d& p, const double* ang, int leg) -{ - if(leg < 2){ - p = f_body_to_shoulder + - (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2])) - .rotate_x(ang[1]).rotate_y(-ang[0]); - }else{ - p = h_body_to_shoulder + - (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2])) - .rotate_x(ang[1]).rotate_y( ang[0]); +LegOrder_t +Kinematics::findUnusedLeg(const NEWMAT::ColumnVector& down) { + float height[NumLegs]; //not actually the real height, but proportional to it, which is all we need + for(unsigned int i=0; iheight[i]) + highleg=i; + //cout << "High: " << highleg << endl; + return static_cast(highleg); } - -void GetBodyLocation(vector3d &ball,vector3d &toe,const double *ang,int leg) -// project various parts of foot that could touch by given joint angles -{ - if(leg < 2){ - ball = f_body_to_shoulder + - (f_leg_shoulder_to_knee + f_leg_knee_to_ball.rotate_y(-ang[2])) - .rotate_x(ang[1]).rotate_y(-ang[0]); - }else{ - ball = h_body_to_shoulder + - (h_leg_shoulder_to_knee + h_leg_knee_to_ball.rotate_y( ang[2])) - .rotate_x(ang[1]).rotate_y( ang[0]); - } - - // TODO: toes - toe = ball; - - if(leg % 2){ - ball.y = -ball.y; - toe.y = -toe.y; - } +NEWMAT::ReturnMatrix +Kinematics::calculateGroundPlane(const NEWMAT::ColumnVector& down) { + //Find the unused foot + unsigned int highleg=findUnusedLeg(down); + + //Fit a plane to the remaining 3 feet + NEWMAT::Matrix legs(3,3); + for(unsigned int i=0; i neck_to_camera.z && target.length() > neck_to_camera.length()) { - double angle_base_target; // angle from base of neck to target xz - double angle_base_target_camera; // angle between base and camera from target xz - - angle_base_target = atan2(target.z,target.x); - angle_base_target_camera = asin(neck_to_camera.z/target_xz_dist); - - tilt = angle_base_target - angle_base_target_camera + body_angle; - tilt = bound(tilt,head_tilt_min,head_tilt_max); - - double camera_dist_to_target; - camera_dist_to_target = sqrt(target_xz_dist*target_xz_dist - - neck_to_camera.z*neck_to_camera.z); - - pan = atan2(target.y,camera_dist_to_target); - pan = bound(pan,head_pan_min,head_pan_max); + /*! Mathematcal implementation: + * + * Need to convert @a p_b to @a p_j + * + * Once we have the transformation Tb_j from b to j, we need:\n + * T2=inv(Tb_j)'; T2(3,1:end-1)=-T2(3,1:end-1);\n + * but since we know a few things about the structure of T, + * we don't have to explicitly calculate that inverse. */ + NEWMAT::Matrix T2=linkToLink(b,j); + //cout << "Transform b->j:\n"<j:\n"<set_q(state->outputs[tkout],j); + } } -// calculates the pose of the camera -// location = location of camera in robot coordinates relative to point on ground under neck -// direction = unit vector pointing in direction of camera -// up = unit vector pointing in direction of higher on image -// right = unit vector pointing in direction of more right on image -void GetHeadPosition(vector3d &location, vector3d &direction, vector3d &up, vector3d &right, - double *angles, double body_angle, double body_height) -{ - location = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,0.0,0.0)); - - vector3d image_x,image_y,image_z; - - image_x = RunForwardModel(angles, body_angle, body_height, vector3d(0.0,-1.0, 0.0)); - image_y = RunForwardModel(angles, body_angle, body_height, vector3d(0.0, 0.0, 1.0)); - image_z = RunForwardModel(angles, body_angle, body_height, vector3d(1.0, 0.0, 0.0)); - - direction = image_z - location; - direction = direction.norm(); - - up = image_y - location; - up = up.norm(); - - right = image_x - location; - right = right.norm(); -} /*! @file - * @brief Functions to provide kinematics calculations - * @author CMU RoboSoccer 2001-2002 (Creator) - * - * @verbinclude CMPack_license.txt + * @brief + * @author ejt (Creator) * * $Author: ejt $ * $Name: $ - * $Revision: 1.5 $ + * $Revision: 1.18 $ * $State: Exp $ - * $Date: 2004/02/26 01:02:49 $ + * $Date: 2004/10/17 01:16:10 $ */ Index: Motion/Kinematics.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/Kinematics.h,v retrieving revision 1.8 retrieving revision 1.23 diff -u -d -r1.8 -r1.23 --- Motion/Kinematics.h 26 Feb 2004 01:02:49 -0000 1.8 +++ Motion/Kinematics.h 19 Oct 2004 17:06:31 -0000 1.23 @@ -1,120 +1,326 @@ //-*-c++-*- -//This class is ported from Carnegie Mellon's 2001 Robosoccer entry, and falls under their license: -/*========================================================================= - CMPack'02 Source Code Release for OPEN-R SDK v1.0 - Copyright (C) 2002 Multirobot Lab [Project Head: Manuela Veloso] - School of Computer Science, Carnegie Mellon University - ------------------------------------------------------------------------- - This software is distributed under the GNU General Public License, - version 2. If you do not have a copy of this licence, visit - www.gnu.org, or write: Free Software Foundation, 59 Temple Place, - Suite 330 Boston, MA 02111-1307 USA. This program is distributed - in the hope that it will be useful, but WITHOUT ANY WARRANTY, - including MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. - ------------------------------------------------------------------------- - Additionally licensed to Sony Corporation under the following terms: +#ifndef INCLUDED_Kinematics_h_ +#define INCLUDED_Kinematics_h_ - This software is provided by the copyright holders AS IS and any - express or implied warranties, including, but not limited to, the - implied warranties of merchantability and fitness for a particular - purpose are disclaimed. In no event shall authors be liable for - any direct, indirect, incidental, special, exemplary, or consequential - damages (including, but not limited to, procurement of substitute - goods or services; loss of use, data, or profits; or business - interruption) however caused and on any theory of liability, whether - in contract, strict liability, or tort (including negligence or - otherwise) arising in any way out of the use of this software, even if - advised of the possibility of such damage. - ========================================================================= -*/ +#include "Shared/RobotInfo.h" +#include "Wireless/Socket.h" +#include "Shared/newmat/newmat.h" +#include +#include +#include -#ifndef __KINEMATICS_H__ -#define __KINEMATICS_H__ +namespace ROBOOP { + class Robot; + class Config; +} -#include "Geometry.h" -#include "Shared/Util.h" +//! Provides access to the mathematical functionality of the ROBOOP package using Tekkotsu data structures +/*! + * You should read the Kinematics + * tutorial to get a general understanding of the math involved + * and diagrams for usage with supported robots. + * + * This class involves all aspects of the forward kinematics: + * calculations concerning locations and orientations in space given + * a known set of joint configurations. There is a global + * instantiation of Kinematics named ::kine, which can be used to + * perform these calculations regarding the joint positions currently + * in ::state. + * + * To perform kinematics on a hypothetical set of joint values, + * please see PostureEngine or its subclasses. PostureEngine also + * includes inverse kinematic functions, which will allow you to + * determine joint angles in order to reach a given point. + * + * The underlying ROBOOP library does not currently handle branching + * kinematic chains - in other words, each limb of the robot is a + * separate Robot as far as ROBOOP is concerned. This class + * interfaces the Tekkotsu array index approach to referencing joints + * with ROBOOP's chain based hierarchy. + * + * Thus, wherever a link or reference frame index is requested, you + * can simply supply one of the output indexes in the usual manner: + * @code kine->frameToBase(HeadOffset+TiltOffset); @endcode + * + * However, there are also a number of points on the body which are + * not joints, but should have their own reference frames, such as + * the base frame, or the camera. These frames have their own + * indices, listed in the robot info file for the model in question + * (such as ERS7Info.h), with names ending in @c FrameOffset. + * @code kine->frameToBase(CameraFrameOffset); @endcode + * + * Since newmat matrix library is used by ROBOOP, you will need to + * pass and receive information in newmat matrices. This class + * provides #pack and #unpack functions which can convert individual + * x,y,z variables into a NEWMAT::ColumnVector, and vice versa. + * However, for readability of your code and long-term ease of use, + * we recommend embracing the newmat data structures directly when + * appropriate. + * @code + * // Find the ray from the camera to whatever the near-field IR is hitting: + * // x and y will be in the range -1 to 1 for resolution layer independence + * // This ignores lens distortion - just proof of concept + * NEWMAT::Matrix T = kine->frameToFrame(NearIRFrameOffset,CameraFrameOffset); + * NEWMAT::ColumnVector camera_ray = T*Kinematics::pack(0,0,state->sensors[NearIRDistOffset]); + * float x=atan(camera_ray(1),camera_ray(3))/config->vision.horizFOV/2; + * float y=atan(camera_ray(2),camera_ray(3))/config->vision.vertFOV/2; + * @endcode + * + * Finally, for each model we have created a database of "interest points", + * locations of notable interest on the body of the robot. These may be of + * use to people attempting to use the limbs to manipulate objects. + * To access these interest points, simply call either #getLinkInterestPoint + * or #getFrameInterestPoint with the name of the interest point, obtained + * from the diagrams. + * + * Note that you can pass a comma separated list of interest point names + * and the result will be the midpoint of those interest points: + * @code kine->getLinkInterestPoint(BaseFrameOffset,"LowerInnerFrontLFrShin,LowerOuterFrontLFrShin"); @endcode + * + * @see PostureEngine for inverse kinematics + */ +class Kinematics { +public: + //!Constructor, pass the full path to the kinematics configuration file + Kinematics() : chains(), chainMaps() { + init(); + } -#define RAD(deg) (((deg) * M_PI ) / 180.0) -#define DEG(rad) (((rad) * 180.0) / M_PI ) + //!Destructor + virtual ~Kinematics() {} -extern const double rotator_min ; -extern const double rotator_max ; -extern const double shoulder_min; -extern const double shoulder_max; -extern const double knee_max ; -extern const double knee_min ; -extern const double rotator_kmin ; -extern const double rotator_kmax ; -extern const double shoulder_kmin; -extern const double shoulder_kmax; -extern const double knee_kmax ; -extern const double f_knee_kmin ; -extern const double h_knee_kmin ; -extern const double tail_min; -extern const double tail_max; + //! Returns a matrix for transforming from link @a j to base frame + /*! @param[in] j the link number, see class notes for values */ + NEWMAT::ReturnMatrix linkToBase(unsigned int j); -extern const double head_tilt_min; -extern const double head_tilt_max; -extern const double head_pan_min ; -extern const double head_pan_max ; -extern const double head_roll_min; -extern const double head_roll_max; + //! Returns a matrix for transforming from frame @a j to base frame + /*! @param[in] j the frame number, see class notes for values */ + NEWMAT::ReturnMatrix frameToBase(unsigned int j); -//! holds the current location of the body, as a delta from when walking started -/*! @todo get rid of this */ -struct BodyPosition{ - //! constructor - BodyPosition() : loc(), angle() {} - vector3d loc; //!< position of the center of the body - vector3d angle; //!< angle of the center of the body -}; + //! Returns a matrix for transforming from the base frame to link @a j + /*! @param[in] j the link number, see class notes for values */ + NEWMAT::ReturnMatrix baseToLink(unsigned int j); -const vector3d body_to_neck ( 75.00, 0.00, 50.00); -const vector3d neck_to_camera ( 65.00, 0.00, 48.00); + //! Returns a matrix for transforming from the base frame to frame @a j + /*! @param[in] j the frame number, see class notes for values */ + NEWMAT::ReturnMatrix baseToFrame(unsigned int j); -void KinClearErrors(); -int KinGetErrors(); + //! Returns a matrix for transforming from link @a ij to link @a oj frame + /*! @param[in] ij the link number to convert from, see class notes for values + * @param[in] oj the link number to convert to, see class notes for values */ + NEWMAT::ReturnMatrix linkToLink(unsigned int ij, unsigned int oj); -void GetLegAngles(double *angles,vector3d target,int leg); -void GetLegAngles(double *angles,vector3d target[4], - double body_angle,double body_height); -void GetLegAngles(double *angles,vector3d target[4],BodyPosition &bp); + //! Returns a matrix for transforming from link @a ij to frame @a oj + /*! @param[in] ij the link number to convert from, see class notes for values + * @param[in] oj the frame number to convert to, see class notes for values */ + NEWMAT::ReturnMatrix linkToFrame(unsigned int ij, unsigned int oj); -void GetLegAngles(double *angles,vector3d target,BodyPosition &bp,int leg); -void GetLegPosition(vector3d& p,const double* ang,int leg); -void GetLegPosition(vector3d& p, const double* ang, BodyPosition &bp,int leg); + //! Returns a matrix for transforming from frame @a ij to link @a oj + /*! @param[in] ij the frame number to convert from, see class notes for values + * @param[in] oj the link number to convert to, see class notes for values */ + NEWMAT::ReturnMatrix frameToLink(unsigned int ij, unsigned int oj); + + //! Returns a matrix for transforming from frame @a ij to frame @a oj frame + /*! @param[in] ij the frame number to convert from, see class notes for values + * @param[in] oj the frame number to convert to, see class notes for values */ + NEWMAT::ReturnMatrix frameToFrame(unsigned int ij, unsigned int oj); + + + + //! Returns the location of a named point and the link it is attached to + /*! @param[in] name the name of the interest point; varies by model, see the diagrams for your model. + * @param[out] j on exit, joint index of the link, or -1U if not found + * @param[out] ip on exit, a homogenous column vector of the requested point + * + * If @a name is not found, j will be -1 and ip will be all 0's. */ + void getInterestPoint(const std::string& name, unsigned int& j, NEWMAT::Matrix& ip); + + //! Returns the location of a named point, relative to any desired reference frame + /*! @param[in] frame the desired reference frame to give results in + * @param[in] name the name of the interest point; varies by model, see the diagrams for your model. + * + * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ + NEWMAT::ReturnMatrix getFrameInterestPoint(unsigned int frame, const std::string& name); + + //! Returns the location of a named point, relative to any desired reference frame + /*! @param[in] link the desired link frame to give results in + * @param[in] name the name of the interest point; varies by model, see the diagrams for your model. + * + * You can pass a comma separated list of interest point names and the result will be the midpoint of those IPs */ + NEWMAT::ReturnMatrix getLinkInterestPoint(unsigned int link, const std::string& name) { + NEWMAT::ColumnVector p=frameToLink(link,link)*getFrameInterestPoint(link,name); + p.Release(); return p; + } -void GetBodyLocation(vector3d &ball,vector3d &toe,const double *ang,int leg); -// get the tilt,pan,roll angles to point the head towards the target assuming -// the given body_angle/body_height -void GetHeadAngles(double *angles,vector3d target, - double body_angle,double body_height); -// converts the camera relative position "point" to a robot centric (under base of neck) position -// using the given head angles (tilt/pan/roll) and body_angle and body_height -vector3d RunForwardModel(double *angles, - double body_angle, double body_height, - vector3d point); -// gets the location of the camera and basis vectors corresponding to the directions of the camera's -// z,-y,x corrdinate axis -void GetHeadPosition(vector3d &location, vector3d &direction, vector3d &up, vector3d &right, - double *angles, double body_angle, double body_height); + //! Find the leg which is in least contact with ground (as best we can anyway) + /*! This can be either based on gravity vector from accelerometer readings, + * or if that may be unreliable due to being in motion, we could do some basic + * balance modeling. + * @return index of leg which is highest in reference to gravity vector */ + LegOrder_t findUnusedLeg(const NEWMAT::ColumnVector& down); + + //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) + /*! This function merely calls the other version of calculateGroundPlane with the current + * gravity vector as the "down" vector. + * @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ + NEWMAT::ReturnMatrix calculateGroundPlane(); + + //! Find the ground plane (by fitting plane to legs other the one specified by findUnusedLeg(down)) + /*! @return vector of the form @f$p_1x + p_2y + p_3z = p_4@f$, relative to the base frame */ + NEWMAT::ReturnMatrix calculateGroundPlane(const NEWMAT::ColumnVector& down); + + //! Find the point of intersection between a ray and a plane + /*! @param j is the link number the ray is relative to + * @param r_j is the line through the origin, in homogenous coordinates + * @param b is the link number the plane is relative to (probably BaseFrameOffset) + * @param p_b represents the plane to be intersected + * @param f is the link number the results should be relative to + * + * @a p_b should be of the form @f$p_1x + p_2y + p_3z = p_4@f$ + * @return homogenous coordinate of intersection (may be infinity) */ + NEWMAT::ReturnMatrix projectToPlane(unsigned int j, const NEWMAT::ColumnVector& r_j, + unsigned int b, const NEWMAT::ColumnVector& p_b, + unsigned int f); + + + //! A simple utility function, converts x,y,z,h to a NEWMAT::ColumnVector + /*! @param[in] x the value for the first row + * @param[in] y the value for the second row + * @param[in] z the value for the third row + * @param[in] h the value for the fourth row (defaults to 1 if not specified) + * @return @f$ \left[\begin{array}{c} x\\y\\z\\h\\ \end{array}\right] @f$ */ + static NEWMAT::ReturnMatrix pack(float x, float y, float z, float h=1) { + NEWMAT::ColumnVector m(4); + m(1)=x; m(2)=y; m(3)=z; m(4)=h; + m.Release(); return m; + } + //! A simple utility function, pulls the first 3 rows of the first column, divides each by the fourth row, and stores into ox, oy, and oz + /*! @param[in] m the matrix to unpack (only uses first column) + * @param[out] ox set to the first row of the first column of @a m, divided by fourth row + * @param[out] oy set to the second row of the first column of @a m, divided by fourth row + * @param[out] oz set to the third row of the first column of @a m, divided by fourth row */ + static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz) { + ox=m(1,1)/m(4,1); oy=m(2,1)/m(4,1); oz=m(3,1)/m(4,1); + } + //! A simple utility function, pulls the first 4 rows of the first column, stores into ox, oy, oz, oh + /*! @param[in] m the matrix to unpack (only uses first column) + * @param[out] ox set to the first row of the first column of @a m + * @param[out] oy set to the second row of the first column of @a m + * @param[out] oz set to the third row of the first column of @a m + * @param[out] oh set to the fourth row of the first column of @a m */ + static void unpack(NEWMAT::Matrix m, float& ox, float& oy, float& oz, float& oh) { + ox=m(1,1); oy=m(2,1); oz=m(3,1); oh=m(4,1); + } + + static ROBOOP::Config * getConfig() { return roconfig; } + +protected: + //! Called by constructors to do basic setup - first call will read Config::motion_config::kinematics from disk, future initializes reuse static roconfig + void init(); + + //! Returns the location of a named point, relative to the link it is attached to + /*! @param[in] name the name of the interest point; varies by model, see the diagrams for your model. + * @param[out] c on exit, chain index the IP is on + * @param[out] l on exit, link index the IP is on + * @param[out] ip on exit, a homogenous column vector of the requested point + * + * If @a name is not found, @a c and @a l will be -1 and @a ip will be all 0's. + * This internal version of the function allows us to use @a c and @a l, ourselves, + * but users will probably want to use the getInterestPoint(name,j,ip) version */ + void getInterestPoint(const std::string& name, unsigned int& c, unsigned int& l, NEWMAT::Matrix& ip); + + //! Called at the beginning of each function which accesses ROBOOP computations - should make sure the ROBOOP structures are up to date with Tekkotsu structures + /*! This class will pull current values from WorldState, but it is expected + * that subclasses (i.e. PostureEngine) will want to provide their own joint values. + * Updates from link 1 through link @a l. + * @param[in] c the chain to update + * @param[in] l the last link to update (later links in the chain are left untouched) */ + virtual void update(unsigned int c, unsigned int l); + + //! converts a Tekkotsu output index to a chain and link + /*! @param[in] tkout the tekkotsu index to lookup + * @param[out] c set to the chain index that @a tkout is in + * @param[out] l set to the link in @a c corresponding to @a tkout */ + bool lookup(unsigned int tkout, unsigned int& c, unsigned int& l) { + if(tkout>=NumReferenceFrames) { + serr->printf("ERROR Kinematics: illegal link %d\n",l); + return false; + } + if(jointMaps[tkout].chain>=chains.size()) { + serr->printf("ERROR Kinematics: no chain available for link %d\n",l); + return false; + } + c=jointMaps[tkout].chain; + l=jointMaps[tkout].link; + return true; + } + + //! A separate ROBOOP::Robot instantiation for each chain since ROBOOP package doesn't support branching chains (which would be rather difficult to implement well) + std::vector chains; + + //! holds mapping for each chain's links back to the tekkotsu outputs they represent + std::vector< std::vector > chainMaps; + + //! Allows mapping from tekkotsu output index to chain and link indicies + struct JointMap { + JointMap() : chain(-1U), link(-1U) {} //!< constructor + JointMap(unsigned int c, unsigned int l) : chain(c), link(l) {} //!< constructor + unsigned int chain; //! the chain index + unsigned int link; //!< the link index + }; + //! holds the position and attached link of a given interest point + struct InterestPoint { + InterestPoint() : x(), y(), z(), chain(), link() {} //!< constructor + InterestPoint(float x_, float y_, float z_, unsigned int chain_, unsigned int link_) + : x(x_), y(y_), z(z_), chain(chain_), link(link_) {} //!< constructor + float x; //!< x value + float y; //!< y value + float z; //!< z value + unsigned int chain; //!< chain containing @a link + int link; //!< link in @a chain + }; + + //! holds mapping from tekkotsu output index to chain and link indicies + JointMap jointMaps[NumReferenceFrames]; + + //! cache of the configuration of the robot for rapid initialization (so we don't have to re-read from disk) + static ROBOOP::Config * roconfig; + //! allows us to use the STL hash_map with strings + struct hashstring { + //! hashes a string by multiplying current total by 5, add new character + /*! not my idea, this is what the STL library does for char*, i've just reimplemented it for strings */ + size_t operator()(const string& s) const { + unsigned long h=0; + for(string::size_type x=s.size(); x!=0; x--) + h=h*5+s[x]; + return (size_t)h; + } + }; + //! we'll be using the hash_map to store named interest points + typedef __gnu_cxx::hash_map InterestPointMap; + //! these interest points are shared by all Kinematics classes (i.e. all PostureEngines) + /*! this is to reduce initialization time, but does mean one robot can't do + * kinematic calculations regarding a different model robot... */ + static InterestPointMap * ips; +}; + +//! a global instance of Kinematics, joint values reference those of WorldState so users can easily query the current spatial locations of joints +extern Kinematics * kine; /*! @file - * @brief Functions to provide kinematics calculations - * @author CMU RoboSoccer 2001-2002 (Creator) - * - * @verbinclude CMPack_license.txt + * @brief Describes Kinematics, which provides access to the mathematical functionality of the roboop package using Tekkotsu data structures + * @author ejt (Creator) * * $Author: ejt $ * $Name: $ - * $Revision: 1.8 $ + * $Revision: 1.23 $ * $State: Exp $ - * $Date: 2004/02/26 01:02:49 $ + * $Date: 2004/10/19 17:06:31 $ */ #endif -// __KINEMATICS_H__ Index: Motion/LedEngine.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/LedEngine.cc,v retrieving revision 1.12 retrieving revision 1.15 diff -u -d -r1.12 -r1.15 --- Motion/LedEngine.cc 3 Feb 2004 01:17:57 -0000 1.12 +++ Motion/LedEngine.cc 12 Sep 2004 04:22:37 -0000 1.15 @@ -8,29 +8,62 @@ /*! This is "Mimic the number" style */ const LEDBitMask_t LedEngine::ERS210numMasks[11] = { ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopBrLEDMask, //0 + ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask, //1 + ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //2 - ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //3 + + ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::TopLLEDMask + |ERS210Info::TopBrLEDMask, //3 + ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask, //4 + ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask, //5 - ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask, //6 + + ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask + |ERS210Info::TopRLEDMask|ERS210Info::TopBrLEDMask, //6 + ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //7 - ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //8 - ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //9 + + ERS210Info::BotRLEDMask|ERS210Info::BotLLEDMask|ERS210Info::MidRLEDMask|ERS210Info::MidLLEDMask + |ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask|ERS210Info::TopBrLEDMask, //8 + + ERS210Info::BotLLEDMask|ERS210Info::MidLLEDMask|ERS210Info::TopRLEDMask|ERS210Info::TopLLEDMask + |ERS210Info::TopBrLEDMask, //9 + ERS210Info::BotLLEDMask //. }; /*! This is "Count the dots" style */ const LEDBitMask_t LedEngine::ERS220numMasks[11] = { ERS220Info::ModeLEDMask, //0 + ERS220Info::FaceBackLeftLEDMask, //1 + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask, //2 + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask, //3 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask, //4 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask, //5 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask, //6 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask, //7 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask, //8 - ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask|ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask|ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask, //9 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask, //4 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask, //5 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask, //6 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask + |ERS220Info::FaceFrontALEDMask, //7 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask + |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask, //8 + + ERS220Info::FaceBackLeftLEDMask|ERS220Info::FaceCenterLeftLEDMask|ERS220Info::FaceFrontLeftLEDMask + |ERS220Info::FaceFrontRightLEDMask|ERS220Info::FaceCenterRightLEDMask|ERS220Info::FaceBackRightLEDMask + |ERS220Info::FaceFrontALEDMask|ERS220Info::FaceFrontBLEDMask|ERS220Info::FaceFrontCLEDMask, //9 + ERS220Info::FaceFrontLeftLEDMask //. }; /* @@ -53,19 +86,37 @@ const LEDBitMask_t LedEngine::ERS7numMasks[11] = { 0, //0 (ERS7Info::FaceLEDPanelMask<<11), //1 + (ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<< 5), //2 + (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<<11)|(ERS7Info::FaceLEDPanelMask<< 3), //3 - (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9), //4 - (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //5 - (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7), //6 - (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<<11), //7 - (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9), //8 - (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4)|(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7)|(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //9 + + (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8) + |(ERS7Info::FaceLEDPanelMask<<9), //4 + + (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 8) + |(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //5 + + (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4) + |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7), //6 + + (ERS7Info::FaceLEDPanelMask<< 0)|(ERS7Info::FaceLEDPanelMask<< 1)|(ERS7Info::FaceLEDPanelMask<< 4) + |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7) + |(ERS7Info::FaceLEDPanelMask<<11), //7 + + (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4) + |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7) + |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9), //8 + + (ERS7Info::FaceLEDPanelMask<< 2)|(ERS7Info::FaceLEDPanelMask<< 3)|(ERS7Info::FaceLEDPanelMask<< 4) + |(ERS7Info::FaceLEDPanelMask<<5)|(ERS7Info::FaceLEDPanelMask<< 6)|(ERS7Info::FaceLEDPanelMask<< 7) + |(ERS7Info::FaceLEDPanelMask<< 8)|(ERS7Info::FaceLEDPanelMask<<9)|(ERS7Info::FaceLEDPanelMask<<11), //9 + (ERS7Info::FaceLEDPanelMask<< 1) //. }; -LedEngine::LedEngine() : numCycling(0), dirty(true), dirtyTime((unsigned int)-1) { +LedEngine::LedEngine() : dirty(true), numCycling(0), nextFlashEnd((unsigned int)-1) { for(unsigned int i=0; it && nextFlashEnd>infos[i].flashtime) + nextFlashEnd=infos[i].flashtime; +} + int LedEngine::isDirty() { unsigned int t = get_time(); - if(t>dirtyTime) { - dirty=true; - dirtyTime=(unsigned int)-1; - for(unsigned int i=0; it && dirtyTime>infos[i].flashtime) - dirtyTime=infos[i].flashtime; - } - return (dirty || numCycling>0); + if(t>nextFlashEnd) { + dirty=true; + recalcFlashEnd(); + }; + return dirty; } int LedEngine::updateLEDs(const MotionCommand* caller, LEDBitMask_t mask/*=AllLEDMask*/) { unsigned int t = get_time(); + if (t>nextFlashEnd) recalcFlashEnd(); for(unsigned int i=0; i>i)&1) for(unsigned int f=0; fsetOutput(caller, i+LEDOffset,calcValue(i,t+f*FrameTime),f); bool tmp=dirty; - dirty=false; + dirty = numCycling>0; return tmp; } int LedEngine::updateLEDs(OutputCmd cmds[NumLEDs]) { unsigned int t = get_time(); + if (t>nextFlashEnd) recalcFlashEnd(); for(unsigned int i=0; i0; return tmp; } int LedEngine::updateLEDFrames(OutputCmd cmds[NumLEDs][NumFrames]) { unsigned int t = get_time(); + if (t>nextFlashEnd) recalcFlashEnd(); for(unsigned int i=0; i0; return tmp; } @@ -142,8 +201,8 @@ void LedEngine::cflash(LEDBitMask_t leds, float value, unsigned int ms) { dirty=true; unsigned int t = get_time(); - if(t+ms>i)&1)*value; infos[i].flashtime=t+ms; @@ -153,8 +212,8 @@ if(leds!=0) { dirty=true; unsigned int t = get_time(); - if(t+ms>i)&1) { infos[i].flashvalue=value; @@ -166,8 +225,8 @@ if(leds!=0) { dirty=true; unsigned int t = get_time(); - if(t+ms>i)&1) { infos[i].flashvalue=calcFlash(calcValue(i,t)); @@ -332,9 +391,9 @@ * @brief Implements LedEngine, which provides basic LED effects to anything that inherits or instantiates it * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.12 $ + * $Revision: 1.15 $ * $State: Exp $ - * $Date: 2004/02/03 01:17:57 $ + * $Date: 2004/09/12 04:22:37 $ */ Index: Motion/LedEngine.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/LedEngine.h,v retrieving revision 1.13 retrieving revision 1.17 diff -u -d -r1.13 -r1.17 --- Motion/LedEngine.h 9 Feb 2004 22:45:28 -0000 1.13 +++ Motion/LedEngine.h 12 Sep 2004 04:22:37 -0000 1.17 @@ -14,8 +14,8 @@ /*! Provides a collection of special effects so that the code can be * reused various places as feedback to to the user. * - * Cycling ("pulsing") and single-value setting are mutually exclusive - * - one will cut off the other + * Cycling ("pulsing") and single-value setting are mutually exclusive; + * one will cut off the other * * A flash will invert and override the current setting, so that it * will "reset" after the flash. Flashes change mid-range values to @@ -31,8 +31,8 @@ * * The ERS-220 and ERS-7 have enough LEDs that they just use a "count * the lights" style of display instead of this pseudo-arabic display. - * (look close to see that green bar LED at the top of the 210, this - * isn't actually too hard to read once you "see" it) + * (look close to see that green bar LED at the top of the 210, which + * doesn't show up well in the camera image for some reason. ) */ class LedEngine { @@ -55,7 +55,10 @@ /*! @param cmds on input, used for weight values - on return, holds the resulting OutputCmd's*/ int updateLEDFrames(OutputCmd cmds[NumLEDs][NumFrames]); - //!returns true if there are changes since the last updateLEDs() + //! recalculates #nextFlashEnd so we can tell when a flash has completed + void recalcFlashEnd(); + + //! returns true if there are changes since the last updateLEDs() int isDirty(); //!sets the leds specified by @a leds to the inverse of their current value @@ -160,20 +163,20 @@ }; LEDInfo infos[NumLEDs]; //!< the information regarding each of the LEDs + bool dirty; //!< true if changes since last updateLEDs: either at least one LED is cycling or a flash has begun/ended unsigned int numCycling;//!< the number of LEDs currently cycling (if non-zero, always dirty) - bool dirty; //!< true if changes since last updateLEDs - unsigned int dirtyTime; //!< the time at which it becomes dirty again (if flashing) + unsigned int nextFlashEnd; //!< the soonest time a flash will end (and we'll become dirty) }; /*! @file * @brief Describes LedEngine, which provides basic LED effects to anything that inherits or instantiates it * @author ejt (Creator) * - * $Author: ejt $ + * $Author: dst $ * $Name: $ - * $Revision: 1.13 $ + * $Revision: 1.17 $ * $State: Exp $ - * $Date: 2004/02/09 22:45:28 $ + * $Date: 2004/09/12 04:22:37 $ */ #endif Index: Motion/LedMC.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/LedMC.h,v retrieving revision 1.6 retrieving revision 1.7 diff -u -d -r1.6 -r1.7 --- Motion/LedMC.h 25 Sep 2003 15:27:23 -0000 1.6 +++ Motion/LedMC.h 10 Sep 2004 22:25:15 -0000 1.7 @@ -2,6 +2,8 @@ #ifndef INCLUDED_LedMC_h #define INCLUDED_LedMC_h +#include "Events/EventBase.h" +#include "Events/EventRouter.h" #include "MotionCommand.h" #include "LedEngine.h" #include "OutputCmd.h" @@ -13,43 +15,52 @@ * just use the engine component to do all the work. */ class LedMC : public MotionCommand, public LedEngine { public: - //! constructor - LedMC() : MotionCommand(), LedEngine() { setWeights(AllLEDMask,1); } - //! destructor - virtual ~LedMC() {} + //! constructor + LedMC() : MotionCommand(), LedEngine(), notified(true) { setWeights(AllLEDMask,1); } + //! destructor + virtual ~LedMC() {} + + //! updates the cmds from LedEngine::updateLEDs() + virtual int updateOutputs() { + updateLEDFrames(cmds); + for(unsigned int i=0; isetOutput(this,i+LEDOffset,cmds[i]); + if (nextFlashEnd < (unsigned int)-1) // do we have a flash in progress? + notified=false; + else if (notified == false) { // flash has ended (nextFlashEnd == -1), but notice not yet sent + postEvent(EventBase(EventBase::motmanEGID,getID(),EventBase::statusETID)); + notified=true; + }; + return NumLEDs; + } - //! updates the cmds from LedEngine::updateLEDs() - virtual int updateOutputs() { - updateLEDFrames(cmds); - for(unsigned int i=0; isetOutput(this,i+LEDOffset,cmds[i]); - return NumLEDs; - } - virtual int isDirty() { return LedEngine::isDirty(); } - virtual int isAlive() { return true; } //!< @todo let's make this smarter so you can flash the LED's and have it autoprune + virtual int isDirty() { return LedEngine::isDirty(); } - //! Sets the JointCmd::weight of the LEDs specified by @a leds to @a weight - void setWeights(LEDBitMask_t leds, float weight) { - for(unsigned int i=0; i>i)&1) - for(unsigned int f=0; f>i)&1) + for(unsigned int f=0; f(reinterpret_cast(sm.data())); if(mc==NULL) { cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl; return invalid_MC_ID; } + mc->setAutoPrune(autoprune); + //cout << "addMotion..."; + while(numAccAddReference(); //cout << "addMotion()NOW: sm.getRegion()->NumberOfReference()==" << sm.getRegion()->NumberOfReference() << endl; cmdlist[mc_id].lastAccessor=_MMaccID; - cmdlist[mc_id].priority=kStdPriority; + cmdlist[mc_id].priority=priority; mc->setAdd(mc_id); OStatus err; /*{ @@ -537,38 +557,6 @@ // cout << "addMotion-done" << endl; return func_end(mc_id); } -MotionManager::MC_ID -MotionManager::addMotion(const SharedObjectBase& sm, float priority) { - func_begin(); - MC_ID mcid=addMotion(sm); - if(mcid!=end()) - setPriority(mcid,priority); - return func_end(mcid); -} -MotionManager::MC_ID -MotionManager::addMotion(const SharedObjectBase& sm, bool autoprune) { - MotionCommand * mc = dynamic_cast(reinterpret_cast(sm.data())); - if(mc==NULL) { - cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl; - return invalid_MC_ID; - } - mc->setAutoPrune(autoprune); - return addMotion(sm); -} -MotionManager::MC_ID -MotionManager::addMotion(const SharedObjectBase& sm, float priority, bool autoprune) { - func_begin(); - MotionCommand * mc = dynamic_cast(reinterpret_cast(sm.data())); - if(mc==NULL) { - cout << "MotionManager::addMotion() - SharedObject does not seem to hold a MotionCommand" << endl; - return invalid_MC_ID; - } - mc->setAutoPrune(autoprune); - MC_ID mcid=addMotion(sm); - if(mcid!=end()) - setPriority(mcid,priority); - return func_end(mcid); -} void MotionManager::receivedMsg(const ONotifyEvent& event) { @@ -588,7 +576,7 @@ //should be able to do a nice dynamic cast instead of a static one // but it gives NULL for some reason - i blame having to do the fork trick cmdlist[mc_id].baseaddrs[_MMaccID]=static_cast(mminfo); - erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,00)); + erouter->postEvent(new EventBase(EventBase::motmanEGID,mc_id,EventBase::activateETID,0)); cmdlist[mc_id].baseaddrs[_MMaccID]->DoStart(); } break; case MotionManagerMsg::deleteMotion: { @@ -650,7 +638,7 @@ func_begin(); checkoutMotion(mcid,true); cmdlist[mcid].baseaddrs[_MMaccID]->DoStop(); - erouter->postEvent(new EventBase(EventBase::motmanEGID,mcid,EventBase::deactivateETID,00)); + erouter->postEvent(new EventBase(EventBase::motmanEGID,mcid,EventBase::deactivateETID,0)); #ifdef PLATFORM_APERIOS MotionManagerMsg dmsg; dmsg.setDelete(mcid); @@ -678,10 +666,10 @@ for(uint u = pidchanges.begin(); u!=pidchanges.end(); u=pidchanges.next(u)) { if(pidchanges[u].joint==joint) { //found it for(uint i=0; i<3; i++) { - pidchanges[i].pids[i]=pids[i]; + pidchanges[u].pids[i]=pids[i]; if(pids[i]!=state->pids[joint][i]) { //see if we're setting back to current PID for(i++; i<3; i++) //we aren't, copy over the rest - pidchanges[i].pids[i]=pids[i]; + pidchanges[u].pids[i]=pids[i]; func_end(); return; } @@ -761,9 +749,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.34 $ + * $Revision: 1.38 $ * $State: Exp $ - * $Date: 2004/02/07 01:02:29 $ + * $Date: 2004/10/17 01:16:11 $ */ Index: Motion/MotionManager.h =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/MotionManager.h,v retrieving revision 1.20 retrieving revision 1.23 diff -u -d -r1.20 -r1.23 --- Motion/MotionManager.h 9 Feb 2004 22:45:28 -0000 1.20 +++ Motion/MotionManager.h 18 Oct 2004 23:10:26 -0000 1.23 @@ -35,7 +35,7 @@ * one line: * * @code - * motman->addMotion(SharedObject([arg1,[arg2,...]]) [, priority [, autoprune] ]); + * motman->addMotion( SharedObject([arg1,...]) , autoprune [, priority ]); * @endcode * * But if you want to do some more initializations not handled by the @@ -47,7 +47,7 @@ * tmpvar->cmd1(); * tmpvar->cmd2(); * //... - * motman->addMotion(tmpvar [, ...]); + * motman->addPrunableMotion(tmpvar [, ...]); //or addPersistentMotion(...) * @endcode * * Notice that tmpvar is of type SharedObject, but you're calling @c @@ -124,14 +124,23 @@ //!@name MotionCommand Unsafe //@{ #ifdef PLATFORM_APERIOS - MC_ID addMotion(const SharedObjectBase& sm); //!< @b LOCKS @b MotionManager Creates a new MotionCommand, automatically sharing it between processes (there is some lag time here) - MC_ID addMotion(const SharedObjectBase& sm, float priority); //!< @b LOCKS @b MotionManager allows a quick way to set a priority level of a new MotionCommand - MC_ID addMotion(const SharedObjectBase& sm, bool autoprune); //!< @b LOCKS @b MotionManager allows a quick was to set the autoprune flag - MC_ID addMotion(const SharedObjectBase& sm, float priority, bool autoprune); //!< @b LOCKS @b MotionManager Call one of these to add a MotionCommand to the MotionManager, using the SharedObject class + //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should be automatically deleted when the MotionCommand::isAlive() returns false. + MC_ID addPrunableMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,true,priority); } + //! @b LOCKS @b MotionManager adds a new motion (wrapped in a SharedObject) and marks that it should @e not be deleted, until removeMotion(MC_ID mcid) is called. + MC_ID addPersistentMotion(const SharedObjectBase& sm, float priority=kStdPriority) { return doAddMotion(sm,false,priority); } #endif //PLATFORM_APERIOS void removeMotion(MC_ID mcid); //!< @b LOCKS @b MotionManager removes the specified MotionCommand //@} + //!@name Deprecated +#ifdef PLATFORM_APERIOS + MC_ID addMotion(const SharedObjectBase& sm) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises + MC_ID addMotion(const SharedObjectBase& sm, bool autoprune) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises + MC_ID addMotion(const SharedObjectBase& sm, float priority) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises + MC_ID addMotion(const SharedObjectBase& sm, float priority, bool autoprune) __attribute__((deprecated)); //!< deprecated, we're recommending users call either addPrunableMotion() or addPersistentMotion() so there are no surprises +#endif //PLATFORM_APERIOS + //@} + //@{ void lock() { MMlock.lock(_MMaccID); } //!< gets an exclusive lock on MotionManager - functions marked @b LOCKS @b MotionManager will cause (and require) this to happen automatically bool trylock() { return MMlock.try_lock(_MMaccID); } //!< tries to get a lock without blocking @@ -166,6 +175,11 @@ }; protected: +#ifdef PLATFORM_APERIOS + //!does the actual work of adding a motion + MC_ID doAddMotion(const SharedObjectBase& sm, bool autoprune, float priority); +#endif + //! used to request pids for a given joint struct PIDUpdate { //!constructor @@ -246,9 +260,9 @@ * * $Author: ejt $ * $Name: $ - * $Revision: 1.20 $ + * $Revision: 1.23 $ * $State: Exp $ - * $Date: 2004/02/09 22:45:28 $ + * $Date: 2004/10/18 23:10:26 $ */ #endif Index: Motion/MotionSequenceMC.cc =================================================================== RCS file: /afs/cs/project/skinnerbots/aibo/Tekkotsu/Motion/MotionSequenceMC.cc,v retrieving revision 1.10 retrieving revision 1.15 diff -u -d -r1.10 -r1.15 --- Motion/MotionSequenceMC.cc 6 Jan 2004 03:19:29 -0000 1.10 +++ Motion/MotionSequenceMC.cc 14 Oct 2004 23:02:53 -0000 1.15 @@ -1,6 +1,7 @@ #include "MotionSequenceMC.h" #include "Shared/get_time.h" #include "Shared/WorldState.h" +#include "Shared/Config.h" #include using std::cout; @@ -67,20 +68,43 @@ unsigned int MotionSequence::LoadBuffer(const char buf[], unsigned int len) { unsigned int origlen=len; - if(strncmp("#MSq\n",buf,5)!=0) { - // cout << "ERROR MotionSequence load corrupted - expected #MSq header" << endl; + if(strncmp("#MSq",buf,4)!=0) { + // we don't want to display an error here because we may be only testing file type, + // so it's up to the caller to decide if it's necessarily an error if the file isn't + // a motion sequence + //cout << "ERROR MotionSequence load corrupted - expected #MSq header" << endl; return 0; } - unsigned int linenum=2; + unsigned int linenum=1; unsigned int lastOutputIdx=0; while(len<=origlen && len>0) { int written; - // printf("%d %.9s\n",linenum+1,buf); - if(buf[0]=='#' || buf[0]=='\n') { - if(strncmp("#END\n",buf,5)==0) - return origlen-len; - else { - while(*buf++!='\n') {} + //printf("%d %.9s\n",linenum+1,buf); + if(buf[0]=='\r') { + buf++; len--; + if(buf[0]=='\n') { + buf++; len--; + } + linenum++; + continue; + } + if(buf[0]=='\n') { + buf++; len--; + linenum++; + continue; + } + if(buf[0]=='#') { + if(strncmp("#END\n",buf,5)==0 || strncmp("#END\r",buf,5)==0) { + return origlen-len+5; + } else if(strncmp("#END\r\n",buf,6)==0) { + return origlen-len+6; + } else { + while(len>0 && *buf!='\n' && *buf!='\r') {len--;buf++;} + if(*buf=='\n') { //in case of \r\n + buf++; + len--; + } + linenum++; continue; } } @@ -97,7 +121,11 @@ written=readWord(buf,&buf[len],arg2,arglen); if(written!=0) if(!ChkAdvance(written,&buf,&len,"*** ERROR MotionSequence load corrupted - line %d\n",linenum)) return 0; - for(;len>0 && *buf!='\n';buf++,len--) {} + for(;len>0 && *buf!='\n' && *buf!='\r';buf++,len--) {} + if(*buf=='\n') { //in case of \r\n + buf++; + len--; + } if(strcasecmp(command,"delay")==0) { char* used; @@ -121,9 +149,9 @@ if(arg1[0]!='/') f="/ms/data/motion/"; f+=arg1; - if(pose.LoadFile(f.c_str())!=0) + if(pose.LoadFile(f.c_str())!=0) { setPose(pose); - else + } else cout << "*** WARNING could not read file " << arg1 << " for load - line " << linenum << endl; } else if(strcasecmp(command,"overlay")==0) { PostureEngine pose; @@ -228,6 +256,13 @@ cout << "SAVE-done!" << endl; } +unsigned int MotionSequence::LoadFile(const char filename[]) { + return LoadSave::LoadFile(config->motion.makePath(filename).c_str()); +} +unsigned int MotionSequence::SaveFile(const char filename[]) const { + return LoadSave::SaveFile(config->motion.makePath(filename).c_str()); +} + void MotionSequence::setPlayTime(unsigned int x) { playtime=x; for(unsigned int i=0; i0) { - *buf+=res; - *len-=res; - return true; - } else { - printf("%s",msg); - return false; - } -} - -bool MotionSequence::ChkAdvance(int res, const char** buf, unsigned int* len, const char* msg, int arg1) { - if(res>0) { - *buf+=res; - *len-=res; - return true; - } else { - printf(msg,arg1); - return false; - } -} - unsigned int MotionSequence::setNextFrameTime(Move_idx_t p[NumOutputs], Move_idx_t n[NumOutputs]) const { unsigned int ans=-1U; for(unsigned int i=0; i' are not meant literally) - * @verbatim - * First line: #MSq - * Zero or more of: delay (moves playhead forward, in milliseconds) - * or: settime