├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── action └── ROS.action ├── data ├── bt_B.txt ├── bt_C.txt ├── bt_baxter.txt ├── bt_bowser.txt ├── bt_default.txt ├── bt_graspthrow.txt ├── bt_luigi.txt ├── bt_mario.txt ├── bt_test.txt └── bt_toad.txt ├── include └── behavior_trees │ ├── display.h │ ├── glutkey.h │ ├── keystroke.h │ ├── navigation.h │ ├── node.h │ ├── parameters.h │ ├── parser.h │ ├── robot_config.h │ └── rosaction.h ├── package.xml ├── scripts ├── io_automata_bt │ ├── io_bt_parser1.py │ ├── io_bt_parser2.py │ ├── sampleBT3.txt │ ├── sampleBT4.txt │ ├── sampleBT5.txt │ ├── sampleBT6.txt │ ├── sampleIO3.txt │ ├── sampleIO4.txt │ ├── sampleIO5.txt │ ├── sampleIO6.txt │ ├── sampleIO7.txt │ ├── testA.txt │ ├── testB.txt │ ├── testC.txt │ ├── testD.txt │ └── testE.txt └── reconfig │ ├── NTUA_init_2015_03.py │ ├── NTUA_init_2015_09.py │ ├── NTUA_init_2016_02.py │ ├── adapt_planner_agent.py │ ├── adapt_planner_agent2.py │ ├── init.py │ ├── nao_static.py │ ├── nao_walker.py │ ├── observeGrasp_OY.py │ ├── planner_agent.py │ ├── poseArmController.py │ ├── poseBaseVSControllerKTH_OY.py │ ├── poseBaseVSControllerKTH_PY.py │ ├── relative_nao.py │ ├── tf2pose.py │ └── tf2pose_for_two.py └── src ├── client ├── display.cpp ├── keystroke.cpp ├── main.cpp ├── navigation.cpp ├── node.cpp ├── parser.cpp ├── robot_config.cpp └── rosaction.cpp └── server └── template_server.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | 6 | # Compiled Dynamic libraries 7 | *.so 8 | *.dylib 9 | 10 | # Compiled Static libraries 11 | *.lai 12 | *.la 13 | *.a 14 | 15 | # temporary files 16 | *~ 17 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(behavior_trees) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | actionlib 9 | actionlib_msgs 10 | message_generation 11 | roscpp 12 | roslib 13 | rospy 14 | std_msgs 15 | ) 16 | 17 | # System dependencies are found with CMake's conventions 18 | find_package(Boost REQUIRED COMPONENTS system thread program_options) 19 | 20 | # Settings 21 | add_definitions(-Wall -g -O0 -Wno-deprecated -static -Bstatic -std=gnu++0x) 22 | 23 | ######################################################### 24 | # FIND GLUT 25 | ######################################################### 26 | find_package(GLUT REQUIRED) 27 | include_directories(${GLUT_INCLUDE_DIRS}) 28 | link_directories(${GLUT_LIBRARY_DIRS}) 29 | add_definitions(${GLUT_DEFINITIONS}) 30 | if(NOT GLUT_FOUND) 31 | message(ERROR " GLUT not found!") 32 | endif(NOT GLUT_FOUND) 33 | 34 | ######################################################### 35 | # FIND OPENGL 36 | ######################################################### 37 | find_package(OpenGL REQUIRED) 38 | include_directories(${OpenGL_INCLUDE_DIRS}) 39 | link_directories(${OpenGL_LIBRARY_DIRS}) 40 | add_definitions(${OpenGL_DEFINITIONS}) 41 | if(NOT OPENGL_FOUND) 42 | message(ERROR " OPENGL not found!") 43 | endif(NOT OPENGL_FOUND) 44 | 45 | 46 | ## Uncomment this if the package has a setup.py. This macro ensures 47 | ## modules and global scripts declared therein get installed 48 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 49 | # catkin_python_setup() 50 | 51 | ################################################ 52 | ## Declare ROS messages, services and actions ## 53 | ################################################ 54 | 55 | ## To declare and build messages, services or actions from within this 56 | ## package, follow these steps: 57 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 58 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 59 | ## * In the file package.xml: 60 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 61 | ## * If MSG_DEP_SET isn't empty the following dependencies might have been 62 | ## pulled in transitively but can be declared for certainty nonetheless: 63 | ## * add a build_depend tag for "message_generation" 64 | ## * add a run_depend tag for "message_runtime" 65 | ## * In this file (CMakeLists.txt): 66 | ## * add "message_generation" and every package in MSG_DEP_SET to 67 | ## find_package(catkin REQUIRED COMPONENTS ...) 68 | ## * add "message_runtime" and every package in MSG_DEP_SET to 69 | ## catkin_package(CATKIN_DEPENDS ...) 70 | ## * uncomment the add_*_files sections below as needed 71 | ## and list every .msg/.srv/.action file to be processed 72 | ## * uncomment the generate_messages entry below 73 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 74 | 75 | ## Generate messages in the 'msg' folder 76 | # add_message_files( 77 | # FILES 78 | # Message1.msg 79 | # Message2.msg 80 | # ) 81 | 82 | ## Generate services in the 'srv' folder 83 | # add_service_files( 84 | # FILES 85 | # Service1.srv 86 | # Service2.srv 87 | # ) 88 | 89 | # Generate actions in the 'action' folder 90 | add_action_files( 91 | DIRECTORY action 92 | FILES 93 | ROS.action 94 | ) 95 | 96 | # Generate added messages and services with any dependencies listed here 97 | generate_messages( 98 | DEPENDENCIES 99 | actionlib_msgs std_msgs 100 | ) 101 | 102 | ################################### 103 | ## catkin specific configuration ## 104 | ################################### 105 | ## The catkin_package macro generates cmake config files for your package 106 | ## Declare things to be passed to dependent projects 107 | ## INCLUDE_DIRS: uncomment this if you package contains header files 108 | ## LIBRARIES: libraries you create in this project that dependent projects also need 109 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 110 | ## DEPENDS: system dependencies of this project that dependent projects also need 111 | catkin_package( 112 | INCLUDE_DIRS include 113 | LIBRARIES bt_server 114 | CATKIN_DEPENDS actionlib actionlib_msgs message_generation roscpp roslib rospy std_msgs 115 | # DEPENDS system_lib 116 | DEPENDS Boost 117 | ) 118 | 119 | 120 | # Includes 121 | # set(COMMON_INCLUDES ${PROJECT_SOURCE_DIR}/include) 122 | # include_directories(${COMMON_INCLUDES}) 123 | 124 | ########### 125 | ## Build ## 126 | ########### 127 | 128 | ## Specify additional locations of header files 129 | ## Your package locations should be listed before other locations 130 | include_directories(include) 131 | include_directories( 132 | ${catkin_INCLUDE_DIRS} 133 | ${Boost_INCLUDE_DIRS} 134 | ) 135 | 136 | ## Declare a cpp library 137 | # add_library(behavior_trees 138 | # src/${PROJECT_NAME}/behavior_trees.cpp 139 | # ) 140 | 141 | ## Declare a cpp executable 142 | # add_executable(behavior_trees_node src/behavior_trees_node.cpp) 143 | 144 | file(GLOB_RECURSE SRC ${PROJECT_SOURCE_DIR}/src/client/*.cpp) 145 | 146 | add_executable(${PROJECT_NAME} ${SRC}) 147 | target_link_libraries(${PROJECT_NAME} ${OPENGL_LIBRARIES} ${GLUT_LIBRARY} ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 148 | 149 | add_executable(template_server src/server/template_server.cpp src/client/rosaction.cpp src/client/robot_config.cpp) 150 | target_link_libraries(template_server ${catkin_LIBRARIES} ${Boost_LIBRARIES}) 151 | 152 | add_library(bt_server STATIC ${PROJECT_SOURCE_DIR}/src/client/rosaction.cpp) 153 | 154 | ## Add cmake target dependencies of the executable/library 155 | ## as an example, message headers may need to be generated before nodes 156 | # add_dependencies(behavior_trees_node behavior_trees_generate_messages_cpp) 157 | #add_dependencies(${PROJECT_NAME} ${catkin_EXPORTED_TARGETS}) 158 | #add_dependencies(${PROJECT_NAME} behavior_trees_gencpp) 159 | 160 | add_dependencies(behavior_trees behavior_trees_generate_messages_cpp) 161 | add_dependencies(template_server behavior_trees_generate_messages_cpp) 162 | add_dependencies(bt_server behavior_trees_generate_messages_cpp) 163 | 164 | 165 | ## Specify libraries to link a library or executable target against 166 | # target_link_libraries(behavior_trees_node 167 | # ${catkin_LIBRARIES} 168 | # ) 169 | 170 | ############# 171 | ## Install ## 172 | ############# 173 | 174 | # all install targets should use catkin DESTINATION variables 175 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 176 | 177 | ## Mark executable scripts (Python etc.) for installation 178 | ## in contrast to setup.py, you can choose the destination 179 | # install(PROGRAMS 180 | # scripts/my_python_script 181 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 182 | # ) 183 | 184 | ## Mark executables and/or libraries for installation 185 | # install(TARGETS behavior_trees behavior_trees_node 186 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 187 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 188 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 189 | # ) 190 | 191 | ## Mark cpp header files for installation 192 | # install(DIRECTORY include/${PROJECT_NAME}/ 193 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 194 | # FILES_MATCHING PATTERN "*.h" 195 | # PATTERN ".svn" EXCLUDE 196 | # ) 197 | 198 | ## Mark other files for installation (e.g. launch and bag files, etc.) 199 | # install(FILES 200 | # # myfile1 201 | # # myfile2 202 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 203 | # ) 204 | 205 | ############# 206 | ## Testing ## 207 | ############# 208 | 209 | ## Add gtest based cpp test target and link libraries 210 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_behavior_trees.cpp) 211 | # if(TARGET ${PROJECT_NAME}-test) 212 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 213 | # endif() 214 | 215 | ## Add folders to be run by python nosetests 216 | # catkin_add_nosetests(test) 217 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 2, June 1991 3 | 4 | Copyright (C) 1989, 1991 Free Software Foundation, Inc., 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | Preamble 10 | 11 | The licenses for most software are designed to take away your 12 | freedom to share and change it. By contrast, the GNU General Public 13 | License is intended to guarantee your freedom to share and change free 14 | software--to make sure the software is free for all its users. This 15 | General Public License applies to most of the Free Software 16 | Foundation's software and to any other program whose authors commit to 17 | using it. (Some other Free Software Foundation software is covered by 18 | the GNU Lesser General Public License instead.) You can apply it to 19 | your programs, too. 20 | 21 | When we speak of free software, we are referring to freedom, not 22 | price. Our General Public Licenses are designed to make sure that you 23 | have the freedom to distribute copies of free software (and charge for 24 | this service if you wish), that you receive source code or can get it 25 | if you want it, that you can change the software or use pieces of it 26 | in new free programs; and that you know you can do these things. 27 | 28 | To protect your rights, we need to make restrictions that forbid 29 | anyone to deny you these rights or to ask you to surrender the rights. 30 | These restrictions translate to certain responsibilities for you if you 31 | distribute copies of the software, or if you modify it. 32 | 33 | For example, if you distribute copies of such a program, whether 34 | gratis or for a fee, you must give the recipients all the rights that 35 | you have. You must make sure that they, too, receive or can get the 36 | source code. And you must show them these terms so they know their 37 | rights. 38 | 39 | We protect your rights with two steps: (1) copyright the software, and 40 | (2) offer you this license which gives you legal permission to copy, 41 | distribute and/or modify the software. 42 | 43 | Also, for each author's protection and ours, we want to make certain 44 | that everyone understands that there is no warranty for this free 45 | software. If the software is modified by someone else and passed on, we 46 | want its recipients to know that what they have is not the original, so 47 | that any problems introduced by others will not reflect on the original 48 | authors' reputations. 49 | 50 | Finally, any free program is threatened constantly by software 51 | patents. We wish to avoid the danger that redistributors of a free 52 | program will individually obtain patent licenses, in effect making the 53 | program proprietary. To prevent this, we have made it clear that any 54 | patent must be licensed for everyone's free use or not licensed at all. 55 | 56 | The precise terms and conditions for copying, distribution and 57 | modification follow. 58 | 59 | GNU GENERAL PUBLIC LICENSE 60 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 61 | 62 | 0. This License applies to any program or other work which contains 63 | a notice placed by the copyright holder saying it may be distributed 64 | under the terms of this General Public License. The "Program", below, 65 | refers to any such program or work, and a "work based on the Program" 66 | means either the Program or any derivative work under copyright law: 67 | that is to say, a work containing the Program or a portion of it, 68 | either verbatim or with modifications and/or translated into another 69 | language. (Hereinafter, translation is included without limitation in 70 | the term "modification".) Each licensee is addressed as "you". 71 | 72 | Activities other than copying, distribution and modification are not 73 | covered by this License; they are outside its scope. The act of 74 | running the Program is not restricted, and the output from the Program 75 | is covered only if its contents constitute a work based on the 76 | Program (independent of having been made by running the Program). 77 | Whether that is true depends on what the Program does. 78 | 79 | 1. You may copy and distribute verbatim copies of the Program's 80 | source code as you receive it, in any medium, provided that you 81 | conspicuously and appropriately publish on each copy an appropriate 82 | copyright notice and disclaimer of warranty; keep intact all the 83 | notices that refer to this License and to the absence of any warranty; 84 | and give any other recipients of the Program a copy of this License 85 | along with the Program. 86 | 87 | You may charge a fee for the physical act of transferring a copy, and 88 | you may at your option offer warranty protection in exchange for a fee. 89 | 90 | 2. You may modify your copy or copies of the Program or any portion 91 | of it, thus forming a work based on the Program, and copy and 92 | distribute such modifications or work under the terms of Section 1 93 | above, provided that you also meet all of these conditions: 94 | 95 | a) You must cause the modified files to carry prominent notices 96 | stating that you changed the files and the date of any change. 97 | 98 | b) You must cause any work that you distribute or publish, that in 99 | whole or in part contains or is derived from the Program or any 100 | part thereof, to be licensed as a whole at no charge to all third 101 | parties under the terms of this License. 102 | 103 | c) If the modified program normally reads commands interactively 104 | when run, you must cause it, when started running for such 105 | interactive use in the most ordinary way, to print or display an 106 | announcement including an appropriate copyright notice and a 107 | notice that there is no warranty (or else, saying that you provide 108 | a warranty) and that users may redistribute the program under 109 | these conditions, and telling the user how to view a copy of this 110 | License. (Exception: if the Program itself is interactive but 111 | does not normally print such an announcement, your work based on 112 | the Program is not required to print an announcement.) 113 | 114 | These requirements apply to the modified work as a whole. If 115 | identifiable sections of that work are not derived from the Program, 116 | and can be reasonably considered independent and separate works in 117 | themselves, then this License, and its terms, do not apply to those 118 | sections when you distribute them as separate works. But when you 119 | distribute the same sections as part of a whole which is a work based 120 | on the Program, the distribution of the whole must be on the terms of 121 | this License, whose permissions for other licensees extend to the 122 | entire whole, and thus to each and every part regardless of who wrote it. 123 | 124 | Thus, it is not the intent of this section to claim rights or contest 125 | your rights to work written entirely by you; rather, the intent is to 126 | exercise the right to control the distribution of derivative or 127 | collective works based on the Program. 128 | 129 | In addition, mere aggregation of another work not based on the Program 130 | with the Program (or with a work based on the Program) on a volume of 131 | a storage or distribution medium does not bring the other work under 132 | the scope of this License. 133 | 134 | 3. You may copy and distribute the Program (or a work based on it, 135 | under Section 2) in object code or executable form under the terms of 136 | Sections 1 and 2 above provided that you also do one of the following: 137 | 138 | a) Accompany it with the complete corresponding machine-readable 139 | source code, which must be distributed under the terms of Sections 140 | 1 and 2 above on a medium customarily used for software interchange; or, 141 | 142 | b) Accompany it with a written offer, valid for at least three 143 | years, to give any third party, for a charge no more than your 144 | cost of physically performing source distribution, a complete 145 | machine-readable copy of the corresponding source code, to be 146 | distributed under the terms of Sections 1 and 2 above on a medium 147 | customarily used for software interchange; or, 148 | 149 | c) Accompany it with the information you received as to the offer 150 | to distribute corresponding source code. (This alternative is 151 | allowed only for noncommercial distribution and only if you 152 | received the program in object code or executable form with such 153 | an offer, in accord with Subsection b above.) 154 | 155 | The source code for a work means the preferred form of the work for 156 | making modifications to it. For an executable work, complete source 157 | code means all the source code for all modules it contains, plus any 158 | associated interface definition files, plus the scripts used to 159 | control compilation and installation of the executable. However, as a 160 | special exception, the source code distributed need not include 161 | anything that is normally distributed (in either source or binary 162 | form) with the major components (compiler, kernel, and so on) of the 163 | operating system on which the executable runs, unless that component 164 | itself accompanies the executable. 165 | 166 | If distribution of executable or object code is made by offering 167 | access to copy from a designated place, then offering equivalent 168 | access to copy the source code from the same place counts as 169 | distribution of the source code, even though third parties are not 170 | compelled to copy the source along with the object code. 171 | 172 | 4. You may not copy, modify, sublicense, or distribute the Program 173 | except as expressly provided under this License. Any attempt 174 | otherwise to copy, modify, sublicense or distribute the Program is 175 | void, and will automatically terminate your rights under this License. 176 | However, parties who have received copies, or rights, from you under 177 | this License will not have their licenses terminated so long as such 178 | parties remain in full compliance. 179 | 180 | 5. You are not required to accept this License, since you have not 181 | signed it. However, nothing else grants you permission to modify or 182 | distribute the Program or its derivative works. These actions are 183 | prohibited by law if you do not accept this License. Therefore, by 184 | modifying or distributing the Program (or any work based on the 185 | Program), you indicate your acceptance of this License to do so, and 186 | all its terms and conditions for copying, distributing or modifying 187 | the Program or works based on it. 188 | 189 | 6. Each time you redistribute the Program (or any work based on the 190 | Program), the recipient automatically receives a license from the 191 | original licensor to copy, distribute or modify the Program subject to 192 | these terms and conditions. You may not impose any further 193 | restrictions on the recipients' exercise of the rights granted herein. 194 | You are not responsible for enforcing compliance by third parties to 195 | this License. 196 | 197 | 7. If, as a consequence of a court judgment or allegation of patent 198 | infringement or for any other reason (not limited to patent issues), 199 | conditions are imposed on you (whether by court order, agreement or 200 | otherwise) that contradict the conditions of this License, they do not 201 | excuse you from the conditions of this License. If you cannot 202 | distribute so as to satisfy simultaneously your obligations under this 203 | License and any other pertinent obligations, then as a consequence you 204 | may not distribute the Program at all. For example, if a patent 205 | license would not permit royalty-free redistribution of the Program by 206 | all those who receive copies directly or indirectly through you, then 207 | the only way you could satisfy both it and this License would be to 208 | refrain entirely from distribution of the Program. 209 | 210 | If any portion of this section is held invalid or unenforceable under 211 | any particular circumstance, the balance of the section is intended to 212 | apply and the section as a whole is intended to apply in other 213 | circumstances. 214 | 215 | It is not the purpose of this section to induce you to infringe any 216 | patents or other property right claims or to contest validity of any 217 | such claims; this section has the sole purpose of protecting the 218 | integrity of the free software distribution system, which is 219 | implemented by public license practices. Many people have made 220 | generous contributions to the wide range of software distributed 221 | through that system in reliance on consistent application of that 222 | system; it is up to the author/donor to decide if he or she is willing 223 | to distribute software through any other system and a licensee cannot 224 | impose that choice. 225 | 226 | This section is intended to make thoroughly clear what is believed to 227 | be a consequence of the rest of this License. 228 | 229 | 8. If the distribution and/or use of the Program is restricted in 230 | certain countries either by patents or by copyrighted interfaces, the 231 | original copyright holder who places the Program under this License 232 | may add an explicit geographical distribution limitation excluding 233 | those countries, so that distribution is permitted only in or among 234 | countries not thus excluded. In such case, this License incorporates 235 | the limitation as if written in the body of this License. 236 | 237 | 9. The Free Software Foundation may publish revised and/or new versions 238 | of the General Public License from time to time. Such new versions will 239 | be similar in spirit to the present version, but may differ in detail to 240 | address new problems or concerns. 241 | 242 | Each version is given a distinguishing version number. If the Program 243 | specifies a version number of this License which applies to it and "any 244 | later version", you have the option of following the terms and conditions 245 | either of that version or of any later version published by the Free 246 | Software Foundation. If the Program does not specify a version number of 247 | this License, you may choose any version ever published by the Free Software 248 | Foundation. 249 | 250 | 10. If you wish to incorporate parts of the Program into other free 251 | programs whose distribution conditions are different, write to the author 252 | to ask for permission. For software which is copyrighted by the Free 253 | Software Foundation, write to the Free Software Foundation; we sometimes 254 | make exceptions for this. Our decision will be guided by the two goals 255 | of preserving the free status of all derivatives of our free software and 256 | of promoting the sharing and reuse of software generally. 257 | 258 | NO WARRANTY 259 | 260 | 11. BECAUSE THE PROGRAM IS LICENSED FREE OF CHARGE, THERE IS NO WARRANTY 261 | FOR THE PROGRAM, TO THE EXTENT PERMITTED BY APPLICABLE LAW. EXCEPT WHEN 262 | OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR OTHER PARTIES 263 | PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY OF ANY KIND, EITHER EXPRESSED 264 | OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 265 | MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE. THE ENTIRE RISK AS 266 | TO THE QUALITY AND PERFORMANCE OF THE PROGRAM IS WITH YOU. SHOULD THE 267 | PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF ALL NECESSARY SERVICING, 268 | REPAIR OR CORRECTION. 269 | 270 | 12. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 271 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY AND/OR 272 | REDISTRIBUTE THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, 273 | INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING 274 | OUT OF THE USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED 275 | TO LOSS OF DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY 276 | YOU OR THIRD PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER 277 | PROGRAMS), EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE 278 | POSSIBILITY OF SUCH DAMAGES. 279 | 280 | END OF TERMS AND CONDITIONS 281 | 282 | How to Apply These Terms to Your New Programs 283 | 284 | If you develop a new program, and you want it to be of the greatest 285 | possible use to the public, the best way to achieve this is to make it 286 | free software which everyone can redistribute and change under these terms. 287 | 288 | To do so, attach the following notices to the program. It is safest 289 | to attach them to the start of each source file to most effectively 290 | convey the exclusion of warranty; and each file should have at least 291 | the "copyright" line and a pointer to where the full notice is found. 292 | 293 | {description} 294 | Copyright (C) {year} {fullname} 295 | 296 | This program is free software; you can redistribute it and/or modify 297 | it under the terms of the GNU General Public License as published by 298 | the Free Software Foundation; either version 2 of the License, or 299 | (at your option) any later version. 300 | 301 | This program is distributed in the hope that it will be useful, 302 | but WITHOUT ANY WARRANTY; without even the implied warranty of 303 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 304 | GNU General Public License for more details. 305 | 306 | You should have received a copy of the GNU General Public License along 307 | with this program; if not, write to the Free Software Foundation, Inc., 308 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA. 309 | 310 | Also add information on how to contact you by electronic and paper mail. 311 | 312 | If the program is interactive, make it output a short notice like this 313 | when it starts in an interactive mode: 314 | 315 | Gnomovision version 69, Copyright (C) year name of author 316 | Gnomovision comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 317 | This is free software, and you are welcome to redistribute it 318 | under certain conditions; type `show c' for details. 319 | 320 | The hypothetical commands `show w' and `show c' should show the appropriate 321 | parts of the General Public License. Of course, the commands you use may 322 | be called something other than `show w' and `show c'; they could even be 323 | mouse-clicks or menu items--whatever suits your program. 324 | 325 | You should also get your employer (if you work as a programmer) or your 326 | school, if any, to sign a "copyright disclaimer" for the program, if 327 | necessary. Here is a sample; alter the names: 328 | 329 | Yoyodyne, Inc., hereby disclaims all copyright interest in the program 330 | `Gnomovision' (which makes passes at compilers) written by James Hacker. 331 | 332 | {signature of Ty Coon}, 1 April 1989 333 | Ty Coon, President of Vice 334 | 335 | This General Public License does not permit incorporating your program into 336 | proprietary programs. If your program is a subroutine library, you may 337 | consider it more useful to permit linking proprietary applications with the 338 | library. If this is what you want to do, use the GNU Lesser General 339 | Public License instead of this License. 340 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | behavior_trees 2 | ============== 3 | 4 | Behavior Trees Library for The Robot Operating System (ROS) 5 | -------------------------------------------------------------------------------- /action/ROS.action: -------------------------------------------------------------------------------- 1 | #goal definition 2 | int32 GOAL_ 3 | --- 4 | #result definition 5 | int32 RESULT_ 6 | --- 7 | #feedback 8 | int32 FEEDBACK_ -------------------------------------------------------------------------------- /data/bt_B.txt: -------------------------------------------------------------------------------- 1 | [ 2 | A ActionDispatcher 3 | ] 4 | -------------------------------------------------------------------------------- /data/bt_C.txt: -------------------------------------------------------------------------------- 1 | [ 2 | A ActionDispatcher 3 | ] 4 | -------------------------------------------------------------------------------- /data/bt_baxter.txt: -------------------------------------------------------------------------------- 1 | [* 2 | A bt_status_action_init 3 | A get_bin_pose_action 4 | A move_action 5 | {* 6 | [* 7 | A grasp_item_action 8 | A bt_status_action_successful_grasp 9 | ] 10 | A bt_status_action_fail_grasp 11 | } 12 | A get_bin_pose_action 13 | A move_action 14 | A bt_status_action_finished_pick 15 | ] 16 | -------------------------------------------------------------------------------- /data/bt_bowser.txt: -------------------------------------------------------------------------------- 1 | / 2 | A ActionDispatcher 3 | A BallTrackerBlue 4 | | 5 | -------------------------------------------------------------------------------- /data/bt_default.txt: -------------------------------------------------------------------------------- 1 | [* 2 | A action_name 3 | A action_name 4 | {* 5 | A action_name 6 | A action_name 7 | } 8 | ] 9 | -------------------------------------------------------------------------------- /data/bt_graspthrow.txt: -------------------------------------------------------------------------------- 1 | / 2 | A BallTrackerRed 3 | [* 4 | A Walker 5 | A HandMover 6 | A BallThrower 7 | ] 8 | | 9 | -------------------------------------------------------------------------------- /data/bt_luigi.txt: -------------------------------------------------------------------------------- 1 | [* 2 | A action_name 3 | ] 4 | -------------------------------------------------------------------------------- /data/bt_mario.txt: -------------------------------------------------------------------------------- 1 | [* 2 | A action_name 3 | ] 4 | -------------------------------------------------------------------------------- /data/bt_test.txt: -------------------------------------------------------------------------------- 1 | [ 2 | A action_name 3 | A action_name 4 | ] 5 | -------------------------------------------------------------------------------- /data/bt_toad.txt: -------------------------------------------------------------------------------- 1 | / 2 | A ActionDispatcher 3 | A BallTrackerBlue 4 | | 5 | -------------------------------------------------------------------------------- /include/behavior_trees/display.h: -------------------------------------------------------------------------------- 1 | #ifndef DISPLAY_H_ 2 | #define DISPLAY_H_ 3 | 4 | #include "behavior_trees/node.h" 5 | 6 | #include 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include 12 | 13 | #define WINDOWS_NAME "Behavior Trees" 14 | #define FULLSCREEN false 15 | #define SCREEN_POSITION_X 200 16 | #define SCREEN_POSITION_Y 200 17 | #define SCREEN_WIDTH 1200 18 | #define SCREEN_HEIGHT 600 // (9.0/16) 19 | #define NODE_WIDTH 5 20 | #define NODE_HEIGHT 5 21 | #define CURSOR_WIDTH 2 * NODE_WIDTH 22 | #define CURSOR_HEIGHT 2 * NODE_HEIGHT 23 | #define STATUS_WIDTH 1.5 * NODE_WIDTH 24 | #define STATUS_HEIGHT 1.5 * NODE_HEIGHT 25 | #define SPACE_HEIGHT SCREEN_HEIGHT / 10 26 | #define FIRST_LINE SCREEN_HEIGHT 27 | #define CENTER_ROW SCREEN_WIDTH / 2 28 | 29 | // functions to manage keystrokes (pressed down) 30 | void keyPressed (unsigned char key, int x, int y); 31 | 32 | // functions to manage keystrokes (released ) 33 | void keyUp (unsigned char key, int x, int y); 34 | 35 | // functions to manage special keystrokes (pressed down) 36 | void keySpecial (int key, int x, int y); 37 | 38 | // functions to manage special keystrokes (released) 39 | void keySpecialUp (int key, int x, int y); 40 | 41 | // draw the bt node's current status using a frame (color coded) 42 | void draw_status(GLfloat x, GLfloat y, STATE node_status); 43 | 44 | // draw the cursor where the user is standing on the bt using a big frame (white) 45 | void draw_cursor(GLfloat x, GLfloat y); 46 | 47 | // draw the node itself using a solid square (color coded) 48 | void draw_node(GLfloat x, GLfloat y, NODE_TYPE node_type); 49 | 50 | // draw the edge connecting one node to the other 51 | void draw_connector(GLfloat parent_x, GLfloat parent_y, GLfloat x, GLfloat y); 52 | 53 | // draws the whole tree, cursor, and statuses, calls swap buffers 54 | void draw_all(); 55 | 56 | // initialization of the Glut parameters, projection type, screen size 57 | void initialize(); 58 | 59 | // more general initialization which includes the keyboard listener 60 | void glut_setup(int iArgc, char** cppArgv); 61 | 62 | // process called to process events 63 | void glut_process(); 64 | 65 | // load fonts to display the node names 66 | void init_font(GLuint base, const char* f); 67 | 68 | // print a string in OpenGL 69 | void print_string(GLuint base, const char* s); 70 | 71 | // load font to be used with OpenGL 72 | void my_init(char* f); 73 | 74 | // reshape the contents of the window keeping aspect ratio (disabled / malfunctioning) 75 | void my_reshape(int w, int h); 76 | 77 | // draw string in OpenGL at position x, y 78 | void draw_string(GLfloat x, GLfloat y, const char* word); 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /include/behavior_trees/glutkey.h: -------------------------------------------------------------------------------- 1 | #ifndef GLUT_KEY_H_ 2 | #define GLUT_KEY_H_ 3 | 4 | #define GLUT_KEY_ENTER 13 5 | #define GLUT_KEY_ESC 27 6 | #define GLUT_KEY_SPACE 32 7 | #define GLUT_KEY_BACKSPACE 8 8 | #define GLUT_KEY_COMMA 44 9 | #define GLUT_KEY_DOT 46 10 | #define GLUT_KEY_HIPHEN 45 11 | 12 | #define GLUT_KEY_A 97 13 | #define GLUT_KEY_B 98 14 | #define GLUT_KEY_C 99 15 | #define GLUT_KEY_D 100 16 | #define GLUT_KEY_E 101 17 | #define GLUT_KEY_F 102 18 | #define GLUT_KEY_G 103 19 | #define GLUT_KEY_H 104 20 | #define GLUT_KEY_I 105 21 | #define GLUT_KEY_J 106 22 | #define GLUT_KEY_K 107 23 | #define GLUT_KEY_L 108 24 | #define GLUT_KEY_M 109 25 | #define GLUT_KEY_N 110 26 | #define GLUT_KEY_O 111 27 | #define GLUT_KEY_P 112 28 | #define GLUT_KEY_Q 113 29 | #define GLUT_KEY_R 114 30 | #define GLUT_KEY_S 115 31 | #define GLUT_KEY_T 116 32 | #define GLUT_KEY_U 117 33 | #define GLUT_KEY_V 118 34 | #define GLUT_KEY_W 119 35 | #define GLUT_KEY_X 120 36 | #define GLUT_KEY_Y 121 37 | #define GLUT_KEY_Z 122 38 | 39 | #define GLUT_KEY_0 48 40 | #define GLUT_KEY_1 49 41 | #define GLUT_KEY_2 50 42 | #define GLUT_KEY_3 51 43 | #define GLUT_KEY_4 52 44 | #define GLUT_KEY_5 53 45 | #define GLUT_KEY_6 54 46 | #define GLUT_KEY_7 55 47 | #define GLUT_KEY_8 56 48 | #define GLUT_KEY_9 57 49 | 50 | #endif 51 | -------------------------------------------------------------------------------- /include/behavior_trees/keystroke.h: -------------------------------------------------------------------------------- 1 | #ifndef KEYSTROKE_H_ 2 | #define KEYSTROKE_H_ 3 | 4 | int get_keypressed(); 5 | int process_keypressed(); 6 | 7 | #endif 8 | -------------------------------------------------------------------------------- /include/behavior_trees/navigation.h: -------------------------------------------------------------------------------- 1 | #ifndef NAVIGATION_H_ 2 | #define NAVIGATION_H_ 3 | 4 | #include 5 | 6 | bool navigate_left(); 7 | 8 | bool navigate_right(); 9 | 10 | bool navigate_up(); 11 | 12 | bool navigate_down(); 13 | 14 | void set_node_state(STATE state); 15 | 16 | void reset_overwritten(); 17 | 18 | void reset_node_state(); 19 | 20 | void print_node_info(); 21 | 22 | #endif 23 | -------------------------------------------------------------------------------- /include/behavior_trees/node.h: -------------------------------------------------------------------------------- 1 | #ifndef NODE_H_ 2 | #define NODE_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | 13 | enum NODE_TYPE 14 | { 15 | SELECTOR, 16 | SELECTOR_STAR, 17 | SEQUENCE, 18 | SEQUENCE_STAR, 19 | PARALLEL, 20 | DECORATOR, 21 | ACTION, 22 | CONDITION, 23 | ROOT 24 | }; 25 | 26 | enum STATE 27 | { 28 | FAILURE = 0, 29 | SUCCESS = 1, 30 | RUNNING = 2, 31 | NODE_ERROR = 3 32 | }; 33 | 34 | class Node 35 | { 36 | protected: 37 | int depth_; 38 | int number_children_; // number of children that this node has e.g. (this node has N children) 39 | int children_number_; // number of this children among its brothers e.g. (index from 0 to N-1 children) 40 | bool highlighted_; 41 | bool overwritten_; 42 | STATE overwritten_result_; 43 | STATE node_status_; 44 | STATE child_status_; 45 | Node *first_child_; 46 | Node *curr_child_; 47 | Node *exec_child_; 48 | Node *next_brother_; 49 | Node *prev_brother_; 50 | Node *parent_; 51 | 52 | public: 53 | 54 | // constructor for the root node 55 | Node(); 56 | 57 | // constructor for any node but root 58 | Node(Node* parent); 59 | 60 | // set node status to default and its children too (recursive) 61 | void execute_reset_status(); 62 | 63 | // virtual functions 64 | virtual STATE execute() =0; 65 | 66 | virtual NODE_TYPE get_node_type() =0; 67 | 68 | virtual std::string get_node_name() =0; 69 | 70 | virtual void set_ros_node_name(std::string name) {} 71 | 72 | virtual std::string get_ros_node_name() { return "error"; } 73 | 74 | // add child distinguishing whether it is the first one or not 75 | Node* add_child(Node* my_child); 76 | 77 | // if it is not the first child, it will be treated as a brother instead 78 | Node* add_brother(Node* my_brother, int children_number); 79 | 80 | // prints the depth, status, and number of children of a certain node 81 | void print_info(); 82 | 83 | // prints the information of the whole tree recursively calling itself 84 | void print_subtree(); 85 | 86 | // draws the tree in OpenGL trying to spread the nodes over the screen 87 | void draw_subtree(GLfloat parent_x, GLfloat parent_y, int number_children, 88 | GLfloat parent_space); 89 | 90 | // inlined functions 91 | inline int get_depth() { return depth_; } 92 | 93 | inline int get_number_children() { return number_children_; } 94 | 95 | inline int get_children_number() { return children_number_; } 96 | 97 | inline Node* get_parent() { return parent_; } 98 | 99 | inline Node* get_first_child() { return first_child_; } 100 | 101 | inline Node* get_next_brother() { return next_brother_; } 102 | 103 | inline Node* get_prev_brother() { return prev_brother_; } 104 | 105 | inline void set_children_number(int number) { children_number_ = number; } 106 | 107 | inline void set_next_brother(Node* pointer) { next_brother_ = pointer; } 108 | 109 | inline void set_prev_brother(Node* pointer) { prev_brother_ = pointer; } 110 | 111 | inline void set_highlighted(bool boolean) { highlighted_ = boolean; } 112 | 113 | inline void set_overwrite(STATE state, bool overwritten) 114 | { overwritten_result_ = state; overwritten_ = overwritten; } 115 | }; 116 | 117 | /* -------------------------------------------------------------------------- */ 118 | /* ----------------------------Basic Nodes----------------------------------- */ 119 | /* -------------------------------------------------------------------------- */ 120 | 121 | class NodeSelector : public Node 122 | { 123 | public: 124 | NodeSelector(Node* node); 125 | private: 126 | STATE execute(); 127 | inline NODE_TYPE get_node_type() { return SELECTOR; } 128 | inline std::string get_node_name() { return "Selector"; } 129 | }; 130 | 131 | class NodeSequence : public Node 132 | { 133 | public: 134 | NodeSequence(Node* node); 135 | private: 136 | STATE execute(); 137 | inline NODE_TYPE get_node_type() { return SEQUENCE; } 138 | inline std::string get_node_name() { return "Sequence"; } 139 | }; 140 | 141 | class NodeParallel : public Node 142 | { 143 | public: 144 | NodeParallel(Node* node); 145 | private: 146 | STATE execute(); 147 | inline NODE_TYPE get_node_type() { return PARALLEL; } 148 | inline std::string get_node_name() { return "Parallel"; } 149 | }; 150 | 151 | class NodeRoot : public Node 152 | { 153 | public: 154 | STATE execute(); 155 | private: 156 | inline NODE_TYPE get_node_type() { return ROOT; } 157 | inline std::string get_node_name() { return "Root"; } 158 | }; 159 | 160 | /* -------------------------------------------------------------------------- */ 161 | /* ----------------------------Star Nodes------------------------------------ */ 162 | /* -------------------------------------------------------------------------- */ 163 | 164 | class NodeSelectorStar : public Node 165 | { 166 | public: 167 | NodeSelectorStar(Node* node); 168 | private: 169 | STATE execute(); 170 | inline NODE_TYPE get_node_type() { return SELECTOR_STAR; } 171 | inline std::string get_node_name() { return "Selector Star"; } 172 | Node *current_running_child_; 173 | }; 174 | 175 | class NodeSequenceStar : public Node 176 | { 177 | public: 178 | NodeSequenceStar(Node* node); 179 | private: 180 | STATE execute(); 181 | inline NODE_TYPE get_node_type() { return SEQUENCE_STAR; } 182 | inline std::string get_node_name() { return "Sequence Star"; } 183 | Node *current_running_child_; 184 | }; 185 | 186 | /* -------------------------------------------------------------------------- */ 187 | /* ------------------------------ROS Nodes----------------------------------- */ 188 | /* -------------------------------------------------------------------------- */ 189 | // typedef actionlib::SimpleActionClient Client; 190 | 191 | class NodeROS : public Node 192 | { 193 | public: 194 | NodeROS(Node* node, std::string name); 195 | 196 | // called once when the goal completes 197 | void doneCb(const actionlib::SimpleClientGoalState& state, 198 | const behavior_trees::ROSResultConstPtr& result); 199 | // called once when the goal becomes active 200 | void activeCb(); 201 | // called every time feedback is received for the goal 202 | void feedbackCb(const behavior_trees::ROSFeedbackConstPtr& feedback); 203 | 204 | private: 205 | inline NODE_TYPE get_node_type() { return ACTION; } 206 | inline std::string get_node_name() { return "Action"; } 207 | inline void set_ros_node_name(std::string name) { ros_node_name_ = name; } 208 | inline std::string get_ros_node_name() { return ros_node_name_; } 209 | STATE execute(); 210 | 211 | std::string ros_node_name_; 212 | bool finished_; 213 | bool received_; 214 | bool active_; 215 | actionlib::SimpleActionClient ac_; 216 | boost::mutex mutex_node_status_; 217 | boost::mutex mutex_finished_; 218 | boost::mutex mutex_received_; 219 | boost::mutex mutex_active_; 220 | }; 221 | 222 | class NodeCondition : public Node 223 | { 224 | public: 225 | NodeCondition(Node* node, std::string varlabel, 226 | std::string relation, std::string constant); 227 | 228 | private: 229 | inline NODE_TYPE get_node_type() { return CONDITION; } 230 | inline std::string get_node_name() { return "Condition"; } 231 | inline void set_condition_name(std::string name) { condition_name_ = name; } 232 | inline std::string get_condition_name() { return condition_name_; } 233 | STATE execute(); 234 | 235 | std::string condition_name_; 236 | std::string varlabel_; 237 | std::string relation_; 238 | std::string constant_; 239 | }; 240 | 241 | class NodeDecorator : public Node 242 | { 243 | public: 244 | NodeDecorator(Node* node, std::string next_state, std::string curr_state, 245 | std::string prev_status); 246 | 247 | private: 248 | inline NODE_TYPE get_node_type() { return DECORATOR; } 249 | inline std::string get_node_name() { return "Decorator"; } 250 | inline void set_decorator_name(std::string name) { decorator_name_ = name; } 251 | inline std::string get_decorator_name() { return decorator_name_; } 252 | STATE execute(); 253 | 254 | std::string decorator_name_; 255 | std::string next_state_; 256 | std::string curr_state_; 257 | std::string prev_status_; 258 | }; 259 | 260 | #endif 261 | 262 | // actionlib::SimpleClientGoalState state = ac_.getState(); 263 | // ROS_INFO("Action finished: %s",state.toString().c_str()); 264 | 265 | // goal.GOAL_ = 1; // possitive tick 266 | // goal.GOAL_ = 0; // neutral tick 267 | // goal.GOAL_ =-1; // negative tick 268 | 269 | // std::cout << "node executing: " << this << std::endl; 270 | // std::cout << "*** ex var: " << node_status_ << std::endl; 271 | -------------------------------------------------------------------------------- /include/behavior_trees/parameters.h: -------------------------------------------------------------------------------- 1 | #ifndef PARAMETERS_H_ 2 | #define PARAMETERS_H_ 3 | 4 | // TIMEOUT_THRESHOLD has to be larger than 1/TICK_FREQUENCY, 5 | // otherwise the server will start and stop all the time since 6 | // the tick frequency is not enough to keep it alive. 7 | 8 | // EXECUTION_FREQUENCY determines the execution rate of the control 9 | // algorithm. It relies on non-blocking functions to be placed inside 10 | // the execute callback. 11 | 12 | #define TICK_FREQUENCY 5.0 // Hz 13 | #define TIMEOUT_THRESHOLD 2.0 // seconds 14 | #define EXECUTION_FREQUENCY 10.0 // Hz 15 | 16 | #endif 17 | -------------------------------------------------------------------------------- /include/behavior_trees/parser.h: -------------------------------------------------------------------------------- 1 | #ifndef PARSER_H_ 2 | #define PARSER_H_ 3 | 4 | #include 5 | #include 6 | #include 7 | 8 | extern std::vector global_varname; 9 | extern std::vector global_varvalue; 10 | 11 | int process_substring(std::string sub); 12 | 13 | int parse_file(std::string name); 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /include/behavior_trees/robot_config.h: -------------------------------------------------------------------------------- 1 | #ifndef ROBOT_CONFIG_H_ 2 | #define ROBOT_CONFIG_H_ 3 | 4 | #include 5 | 6 | namespace po = boost::program_options; 7 | 8 | void setupCmdLineReader(); 9 | void addCmdLineOption(std::string argumentName); 10 | std::string readCmdLineOption(std::string argumentName); 11 | // std::string readRobotIPFromCmdLine(int argc, char** argv); 12 | // std::string readColorFromCmdLine(int argc, char** argv); 13 | std::string readAgentFromCmdLine(int argc, char** argv); 14 | 15 | #endif 16 | -------------------------------------------------------------------------------- /include/behavior_trees/rosaction.h: -------------------------------------------------------------------------------- 1 | #ifndef ROSACTION_H_ 2 | #define ROSACTION_H_ 3 | 4 | #include 5 | #include 6 | 7 | #include "behavior_trees/node.h" 8 | #include "behavior_trees/parameters.h" 9 | 10 | extern bool busy; 11 | 12 | class ROSAction 13 | { 14 | protected: 15 | 16 | ros::NodeHandle nh_; 17 | actionlib::SimpleActionServer as_; 18 | std::string action_name_; 19 | 20 | bool started_; 21 | bool active_; 22 | 23 | int goal_; 24 | behavior_trees::ROSFeedback feedback_; 25 | behavior_trees::ROSResult result_; 26 | 27 | ros::Time start_time_; 28 | ros::Duration elapsed_time_; 29 | 30 | boost::thread execution_thread_; 31 | boost::mutex mutex_started_; 32 | boost::mutex mutex_active_; 33 | boost::mutex mutex_start_time_; 34 | boost::mutex mutex_elapsed_time_; 35 | boost::mutex mutex_feedback_; 36 | boost::mutex mutex_result_; 37 | 38 | public: 39 | 40 | // construct action with: A name 41 | ROSAction(std::string name); 42 | 43 | // destroy action, i.e. default thread 44 | ~ROSAction(void); 45 | 46 | // thread that calls executeCB 47 | void executionThread(); 48 | 49 | // called each time a goal is received 50 | void goalCB(); 51 | 52 | // called each time a goal is preempted 53 | void preemptCB(); 54 | 55 | // start thread 56 | void start(); 57 | 58 | // stop thread 59 | void stop(); 60 | 61 | // activate executeCB 62 | void activate(); 63 | 64 | // deactivate executeCB 65 | void deactivate(); 66 | 67 | // get status of started 68 | bool is_started(); 69 | 70 | // get status of active 71 | bool is_active(); 72 | 73 | // check if no tick was received is TIMEOUT_THRESHOLD 74 | bool timeout_check(); 75 | 76 | // reset because tick was received possibly at TICK_FREQUENCY 77 | void reset_timeout(); 78 | 79 | // send partial feedback 80 | void send_feedback(); 81 | 82 | // send final result 83 | void send_result(); 84 | 85 | // sets the feedback to a certain value before sent in callback 86 | void set_feedback(STATE state); 87 | 88 | // sets the result to a certain value before sent in callback 89 | void set_result(STATE state); 90 | 91 | // function called periodically at EXECUTION_FREQUENCY 92 | virtual int executeCB(ros::Duration dt) { return 0; } 93 | 94 | // called when the thread is started only 95 | virtual void resetCB() {} 96 | }; 97 | 98 | #endif 99 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | behavior_trees 4 | 0.0.0 5 | The behavior_trees package 6 | 7 | 8 | 9 | 10 | marzinotto 11 | 12 | 13 | 14 | 15 | 16 | TODO 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 30 | 31 | 32 | 33 | 34 | 35 | 36 | 37 | 38 | 39 | 40 | 41 | 42 | catkin 43 | actionlib 44 | actionlib_msgs 45 | message_generation 46 | roscpp 47 | roslib 48 | rospy 49 | std_msgs 50 | actionlib 51 | actionlib_msgs 52 | message_generation 53 | roscpp 54 | roslib 55 | rospy 56 | std_msgs 57 | 58 | 59 | 60 | 61 | 62 | 63 | 64 | 65 | 66 | 67 | 68 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/io_bt_parser1.py: -------------------------------------------------------------------------------- 1 | test_letter = "E" 2 | # io_filename = "sampleIO.txt" 3 | io_filename = "test" + test_letter + ".txt" 4 | bt_filename = "../../bt.txt" 5 | f = open(io_filename, 'r') 6 | b = open(bt_filename, 'w') 7 | 8 | n_states = int(f.readline()) 9 | q_init = int(f.readline()) 10 | sigma_in = range(0, 2) # 0: fail, 1: succ 11 | sigma_out = range(1, int(f.readline()) + 1 ) 12 | d_transi = [[ None for x in range(2)] for y in range(n_states)] 13 | d_action = [[ None for x in range(2)] for y in range(n_states)] 14 | # action_l = [ "ActionName" for x in sigma_out] 15 | action_l = [ "MoveRight", "MoveDown", "MoveLeft", "MoveUp", \ 16 | "MoveHand" , "BallThrower", "Cycle"] 17 | print action_l 18 | 19 | for i in range(n_states): 20 | d_transi[i] = map(int, f.readline().split()) 21 | for i in range(n_states): 22 | d_action[i] = map(int, f.readline().split()) 23 | 24 | print n_states 25 | print q_init 26 | print sigma_in 27 | print sigma_out 28 | print d_transi 29 | print d_action 30 | 31 | # Q: state label, P: previous success or failure 32 | b.write('V Q ' + str(q_init) + '\n') 33 | b.write('V P ' + str( 0) + '\n') # assume prev action succeeded 34 | b.write('{\n') 35 | for i in range(n_states): 36 | b.write('\t[*\n') # 37 | b.write('\t\tC Q = ' + str(i+1) + '\n') ## 38 | b.write('\t\t{*\n') ## 39 | b.write('\t\t\t[*\n') ### 40 | b.write('\t\t\t\tC P = ' + str(sigma_in[0]) + '\n') #### 41 | b.write('\t\t\t\td ' + str( d_transi[i][0]) + ' Q P\n') #### 42 | b.write('\t\t\t\t\tA ' + action_l[d_action[i][0]-1] + '\n') ##### 43 | b.write('\t\t\t\tD\n') #### 44 | b.write('\t\t\t]\n') ### 45 | b.write('\t\t\t[*\n') ### 46 | b.write('\t\t\t\tC P = ' + str(sigma_in[1]) + '\n') #### 47 | b.write('\t\t\t\td ' + str( d_transi[i][1]) + ' Q P\n') #### 48 | b.write('\t\t\t\t\tA ' + action_l[d_action[i][1]-1] + '\n') ##### 49 | b.write('\t\t\t\tD\n') #### 50 | b.write('\t\t\t]\n') ### 51 | b.write('\t\t}\n') ## 52 | b.write('\t]\n') # 53 | b.write('}\n') 54 | b.close() 55 | print "file write successful" 56 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleBT3.txt: -------------------------------------------------------------------------------- 1 | { 2 | [ 3 | 1 4 | 2 5 | 3 6 | ] 7 | 4 8 | } 9 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleBT4.txt: -------------------------------------------------------------------------------- 1 | { 2 | 1 3 | [ 4 | 2 5 | 3 6 | 4 7 | ] 8 | } 9 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleBT5.txt: -------------------------------------------------------------------------------- 1 | [ 2 | 1 3 | 2 4 | 3 5 | 4 6 | ] 7 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleBT6.txt: -------------------------------------------------------------------------------- 1 | { 2 | 5 3 | 1 4 | 2 5 | 4 6 | 3 7 | } 8 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleIO3.txt: -------------------------------------------------------------------------------- 1 | 5 2 | 1 3 | 9 4 | 2 2 5 | 3 5 6 | 4 5 7 | 1 5 8 | 1 1 9 | 1 1 10 | 2 4 11 | 3 4 12 | 5 4 13 | 5 5 14 | 15 | % explanation: 16 | % 1st line is n = the number of states in I.Q, i.e. the states are 1,...,n. 17 | % 2nd line is I.qinit, the initial state from 1,...,n 18 | % 3rd line is m = the number of symbols in the output alphabet I.Sigmaout, which are labeled 1,...,m. (1 corresponds e.g. to go_straight action, 2 to grab action, etc. -- we have to agree on these, please name them as you wish for now) 19 | % 4th to (4+n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Q; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, move the I/O automaton to the first state, in case of failure, move the I/O automaton to the second state on that line 20 | % 4+1th to (4+2n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Sigmaout; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, output the first action to be performed in T, in case of failure, output the second action given on that line 21 | 22 | %Particular example: Being in the state 2, in case of success of the last action, move to the state 5, output action 9; in case of failure of the last action, move to state 4, output action 8; 23 | 24 | with sel* and seq* the tick never travel backwards, only after the s/f goes back to the root. with sel and seq the tick can travel backwards if the action is preempted but we are not considering that. this means that all existing loops should wrap up exactly at the same poin (the root): if there are two or more loops that wrap up at different places, it is not possible. also, a node looping on itself casting the same action would require something like a decorator, or an infinitely deep tree / infinitely long sequence of the same action. 25 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleIO4.txt: -------------------------------------------------------------------------------- 1 | 5 2 | 1 3 | 9 4 | 2 2 5 | 1 3 6 | 4 1 7 | 5 1 8 | 1 1 9 | 1 1 10 | s 2 11 | 3 f 12 | 4 f 13 | s f 14 | 15 | % explanation: 16 | % 1st line is n = the number of states in I.Q, i.e. the states are 1,...,n. 17 | % 2nd line is I.qinit, the initial state from 1,...,n 18 | % 3rd line is m = the number of symbols in the output alphabet I.Sigmaout, which are labeled 1,...,m. (1 corresponds e.g. to go_straight action, 2 to grab action, etc. -- we have to agree on these, please name them as you wish for now) 19 | % 4th to (4+n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Q; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, move the I/O automaton to the first state, in case of failure, move the I/O automaton to the second state on that line 20 | % 4+1th to (4+2n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Sigmaout; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, output the first action to be performed in T, in case of failure, output the second action given on that line 21 | 22 | %Particular example: Being in the state 2, in case of success of the last action, move to the state 5, output action 9; in case of failure of the last action, move to state 4, output action 8; 23 | 24 | with sel* and seq* the tick never travel backwards, only after the s/f goes back to the root. with sel and seq the tick can travel backwards if the action is preempted but we are not considering that. this means that all existing loops should wrap up exactly at the same poin (the root): if there are two or more loops that wrap up at different places, it is not possible. also, a node looping on itself casting the same action would require something like a decorator, or an infinitely deep tree / infinitely long sequence of the same action. 25 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleIO5.txt: -------------------------------------------------------------------------------- 1 | 5 2 | 1 3 | 9 4 | 2 2 5 | 3 1 6 | 4 1 7 | 5 1 8 | 1 1 9 | 1 1 10 | 2 f 11 | 3 f 12 | 4 f 13 | s f 14 | 15 | % explanation: 16 | % 1st line is n = the number of states in I.Q, i.e. the states are 1,...,n. 17 | % 2nd line is I.qinit, the initial state from 1,...,n 18 | % 3rd line is m = the number of symbols in the output alphabet I.Sigmaout, which are labeled 1,...,m. (1 corresponds e.g. to go_straight action, 2 to grab action, etc. -- we have to agree on these, please name them as you wish for now) 19 | % 4th to (4+n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Q; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, move the I/O automaton to the first state, in case of failure, move the I/O automaton to the second state on that line 20 | % 4+1th to (4+2n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Sigmaout; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, output the first action to be performed in T, in case of failure, output the second action given on that line 21 | 22 | %Particular example: Being in the state 2, in case of success of the last action, move to the state 5, output action 9; in case of failure of the last action, move to state 4, output action 8; 23 | 24 | with sel* and seq* the tick never travel backwards, only after the s/f goes back to the root. with sel and seq the tick can travel backwards if the action is preempted but we are not considering that. this means that all existing loops should wrap up exactly at the same poin (the root): if there are two or more loops that wrap up at different places, it is not possible. also, a node looping on itself casting the same action would require something like a decorator, or an infinitely deep tree / infinitely long sequence of the same action. 25 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleIO6.txt: -------------------------------------------------------------------------------- 1 | 6 2 | 1 3 | 9 4 | 2 2 5 | 1 3 6 | 1 4 7 | 1 5 8 | 1 6 9 | 1 1 10 | 5 5 11 | s 1 12 | s 2 13 | s 4 14 | s 3 15 | s f 16 | 17 | % explanation: 18 | % 1st line is n = the number of states in I.Q, i.e. the states are 1,...,n. 19 | % 2nd line is I.qinit, the initial state from 1,...,n 20 | % 3rd line is m = the number of symbols in the output alphabet I.Sigmaout, which are labeled 1,...,m. (1 corresponds e.g. to go_straight action, 2 to grab action, etc. -- we have to agree on these, please name them as you wish for now) 21 | % 4th to (4+n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Q; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, move the I/O automaton to the first state, in case of failure, move the I/O automaton to the second state on that line 22 | % 4+1th to (4+2n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Sigmaout; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, output the first action to be performed in T, in case of failure, output the second action given on that line 23 | 24 | %Particular example: Being in the state 2, in case of success of the last action, move to the state 5, output action 9; in case of failure of the last action, move to state 4, output action 8; 25 | 26 | with sel* and seq* the tick never travel backwards, only after the s/f goes back to the root. with sel and seq the tick can travel backwards if the action is preempted but we are not considering that. this means that all existing loops should wrap up exactly at the same poin (the root): if there are two or more loops that wrap up at different places, it is not possible. also, a node looping on itself casting the same action would require something like a decorator, or an infinitely deep tree / infinitely long sequence of the same action. 27 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/sampleIO7.txt: -------------------------------------------------------------------------------- 1 | 13 2 | 1 3 | 12 4 | 2 2 5 | 3 1 6 | 4 1 7 | 5 7 8 | 6 7 9 | 8 7 10 | 8 1 11 | 10 9 12 | 10 1 13 | 13 11 14 | 13 12 15 | 13 1 16 | 1 1 17 | 1 1 18 | 2 f 19 | 3 f 20 | 4 6 21 | 5 6 22 | 7 6 23 | 7 f 24 | 9 8 25 | 9 f 26 | 12 10 27 | 12 11 28 | 12 f 29 | s f 30 | 31 | % explanation: 32 | % 1st line is n = the number of states in I.Q, i.e. the states are 1,...,n. 33 | % 2nd line is I.qinit, the initial state from 1,...,n 34 | % 3rd line is m = the number of symbols in the output alphabet I.Sigmaout, which are labeled 1,...,m. (1 corresponds e.g. to go_straight action, 2 to grab action, etc. -- we have to agree on these, please name them as you wish for now) 35 | % 4th to (4+n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Q; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, move the I/O automaton to the first state, in case of failure, move the I/O automaton to the second state on that line 36 | % 4+1th to (4+2n-1)th lines represent the part of I.delta that is I.Q \times {succ,fail} \rightarrow Sigmaout; it is always given on n lines, two columns. Read ith line as: while being in state i of the I/O automaton, in case of success, output the first action to be performed in T, in case of failure, output the second action given on that line 37 | 38 | %Particular example: Being in the state 2, in case of success of the last action, move to the state 5, output action 9; in case of failure of the last action, move to state 4, output action 8; 39 | 40 | with sel* and seq* the tick never travel backwards, only after the s/f goes back to the root. with sel and seq the tick can travel backwards if the action is preempted but we are not considering that. this means that all existing loops should wrap up exactly at the same poin (the root): if there are two or more loops that wrap up at different places, it is not possible. also, a node looping on itself casting the same action would require something like a decorator, or an infinitely deep tree / infinitely long sequence of the same action. 41 | -------------------------------------------------------------------------------- /scripts/io_automata_bt/testA.txt: -------------------------------------------------------------------------------- 1 | 17 2 | 1 3 | 7 4 | 2 1 5 | 3 2 6 | 4 3 7 | 5 4 8 | 6 5 9 | 7 14 10 | 8 7 11 | 9 8 12 | 10 9 13 | 11 10 14 | 12 11 15 | 13 12 16 | 1 13 17 | 15 14 18 | 16 15 19 | 17 16 20 | 1 17 21 | 2 7 22 | 1 2 23 | 1 1 24 | 2 1 25 | 5 2 26 | 4 4 27 | 3 4 28 | 4 3 29 | 6 4 30 | 2 6 31 | 3 2 32 | 4 3 33 | 7 4 34 | 3 4 35 | 3 3 36 | 4 3 37 | 7 4 -------------------------------------------------------------------------------- /scripts/io_automata_bt/testB.txt: -------------------------------------------------------------------------------- 1 | 13 2 | 1 3 | 7 4 | 2 1 5 | 3 2 6 | 4 3 7 | 5 12 8 | 6 5 9 | 7 6 10 | 8 7 11 | 9 8 12 | 10 9 13 | 11 10 14 | 1 11 15 | 13 12 16 | 1 13 17 | 2 7 18 | 2 2 19 | 5 2 20 | 4 4 21 | 1 4 22 | 4 1 23 | 6 4 24 | 2 6 25 | 3 2 26 | 4 3 27 | 7 4 28 | 4 4 29 | 7 4 -------------------------------------------------------------------------------- /scripts/io_automata_bt/testC.txt: -------------------------------------------------------------------------------- 1 | 25 2 | 1 3 | 7 4 | 2 1 5 | 3 2 6 | 4 3 7 | 5 4 8 | 6 5 9 | 7 6 10 | 8 7 11 | 9 8 12 | 10 9 13 | 11 20 14 | 12 11 15 | 13 12 16 | 14 13 17 | 15 14 18 | 16 15 19 | 17 16 20 | 18 17 21 | 19 18 22 | 1 19 23 | 21 20 24 | 22 21 25 | 23 22 26 | 24 23 27 | 25 24 28 | 1 25 29 | 2 7 30 | 1 2 31 | 4 1 32 | 2 4 33 | 1 2 34 | 4 1 35 | 2 4 36 | 2 2 37 | 5 2 38 | 4 4 39 | 3 4 40 | 2 3 41 | 4 2 42 | 3 4 43 | 2 3 44 | 6 2 45 | 4 6 46 | 4 4 47 | 7 4 48 | 3 4 49 | 2 3 50 | 4 2 51 | 3 4 52 | 4 3 53 | 7 4 -------------------------------------------------------------------------------- /scripts/io_automata_bt/testD.txt: -------------------------------------------------------------------------------- 1 | 19 2 | 1 3 | 7 4 | 2 1 5 | 3 2 6 | 4 3 7 | 5 14 8 | 6 5 9 | 7 6 10 | 8 7 11 | 9 8 12 | 10 9 13 | 11 10 14 | 12 11 15 | 13 12 16 | 1 13 17 | 15 14 18 | 16 15 19 | 17 16 20 | 18 17 21 | 19 18 22 | 1 19 23 | 2 7 24 | 2 2 25 | 5 2 26 | 4 4 27 | 1 4 28 | 2 1 29 | 4 2 30 | 4 4 31 | 6 4 32 | 2 6 33 | 3 2 34 | 4 3 35 | 7 4 36 | 1 4 37 | 2 1 38 | 4 2 39 | 3 4 40 | 4 3 41 | 7 4 -------------------------------------------------------------------------------- /scripts/io_automata_bt/testE.txt: -------------------------------------------------------------------------------- 1 | 24 2 | 1 3 | 7 4 | 2 1 5 | 3 2 6 | 4 3 7 | 5 12 8 | 6 5 9 | 7 6 10 | 8 7 11 | 9 8 12 | 10 9 13 | 11 10 14 | 1 11 15 | 13 12 16 | 14 13 17 | 15 14 18 | 16 22 19 | 17 16 20 | 18 17 21 | 19 18 22 | 20 19 23 | 21 20 24 | 11 21 25 | 23 22 26 | 24 23 27 | 1 24 28 | 2 7 29 | 2 2 30 | 5 2 31 | 4 4 32 | 1 4 33 | 4 1 34 | 6 4 35 | 2 6 36 | 3 2 37 | 4 3 38 | 7 4 39 | 1 4 40 | 2 1 41 | 5 2 42 | 4 4 43 | 4 4 44 | 6 4 45 | 2 6 46 | 3 2 47 | 4 3 48 | 7 4 49 | 3 4 50 | 4 3 51 | 7 4 -------------------------------------------------------------------------------- /scripts/reconfig/NTUA_init_2015_03.py: -------------------------------------------------------------------------------- 1 | from ltl_tools.ts import MotionFts, ActionModel 2 | #======================================================= 3 | # workspace layout for both agents 4 | # pyr2 || oyr2 5 | # pyr3||oyr3 6 | # || 7 | # pyr1 pyr0||oyr0 oyr1 8 | # ====================================================== 9 | init=dict() 10 | #======================================================= 11 | # Model and task for Observing Youbot (OY) 12 | # | oyr2 13 | # |oyr3 14 | # | 15 | # |oyr0 oyr1 16 | ################ OY motion ################## 17 | OY_node=[(0.8,0.72,3.0),(0.89,-0.24,2.6)] 18 | OY_label={ 19 | OY_node[0] : set(['oyr2']), 20 | OY_node[1]: set(['oyr1',]), #oyball 21 | } 22 | OY_init_pose=OY_node[0] 23 | OY_symbols=set(['oyr0','oyr1']) 24 | OY_motion=MotionFts(OY_label,OY_symbols,'OY-workspace') 25 | OY_motion.set_initial(OY_init_pose) 26 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 27 | OY_edge=[(OY_node[0],OY_node[1]),] 28 | OY_motion.add_un_edges(OY_edge,unit_cost=10.0) 29 | ########### OY action ########## 30 | OY_action_label={ 31 | 'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])), 32 | } 33 | OY_action = ActionModel(OY_action_label) 34 | ########### OY task ############ 35 | #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 36 | OY_task = '(<> (oyr1 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)' 37 | ########### OY initialize ############ 38 | init['OY']=(OY_motion, OY_action, OY_task) 39 | 40 | #======================================================= 41 | # Model and task for Pointing Youbot (PY) 42 | # pyr2 | 43 | # pyr3| 44 | # | 45 | # pyr1 pyr0| 46 | ########### PY motion ########## 47 | PY_node=[(-1.3,1.10,0.0),(-1.27,-0.1,0.0),(-0.81,0.35,1.5)] 48 | PY_label={ 49 | PY_node[0]: set(['pyr2']), 50 | PY_node[1]: set(['pyr1',]), 51 | PY_node[2]: set(['pyr3',]), #'pyball' 52 | } 53 | PY_init_pose=PY_node[0] 54 | PY_symbols=set(['pyr1','pyr2','pyr3']) 55 | PY_motion=MotionFts(PY_label,PY_symbols,'PY-workspace') 56 | PY_motion.set_initial(PY_init_pose) 57 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 58 | PY_edge=[(PY_node[0],PY_node[1]),(PY_node[1],PY_node[2]),(PY_node[2],PY_node[0]),] 59 | PY_motion.add_un_edges(PY_edge,unit_cost=10.0) 60 | ########### PY action ########## 61 | PY_action_label={ 62 | 'pypoint': (100, 'pyball', set(['pypoint'])), 63 | } 64 | PY_action = ActionModel(PY_action_label) 65 | ########### PY task ############ 66 | #PY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 67 | PY_task = '(<> (pyr3 && <> pypoint)) && ([]<> pyr1) && ([]<> pyr2) && ([]<> pyr3)' 68 | ########### PY initialize ############ 69 | init['PY']=(PY_motion, PY_action, PY_task) 70 | 71 | 72 | #============================================= 73 | # python adapt_planner_agent2.py PY 74 | # when manually send a message to PY 75 | # ecps 76 | # rostopic pub -1 knowledge_PY ltl3_NTUA/knowledge -- 'pyball' 'pyr3' 77 | # aalto 78 | # rostopic pub -1 observation_done std_msgs/Bool 1 79 | # after PY updates plan, it moves to 'pyr3' to execute its action 'pypoint', 80 | # PY should publish message '['oyball', 'oyr3']' to topic 'knowledge_OY' 81 | # rostopic pub -1 knowledge_OY ltl3_NTUA/knowledge -- 'oyball' 'oyr1' 82 | # then OY would update its plan, and moves to 'oyr3' to execute its action 'oyobsgrasp' 83 | 84 | 85 | #========================================= 86 | # for testing 87 | # rostopic pub -1 activity_done_PY ltl3_NTUA/confirmation -- '0' 'goto' '10' 88 | # rostopic pub -1 activity_done_OY ltl3_NTUA/confirmation -- '0' 'goto' '10' 89 | -------------------------------------------------------------------------------- /scripts/reconfig/NTUA_init_2015_09.py: -------------------------------------------------------------------------------- 1 | from ltl_tools.ts import MotionFts, ActionModel 2 | #======================================================= 3 | # workspace layout for both agents 4 | # naor1 naor2 5 | # pyr2 || oyr2 6 | # pyr3||oyr3 7 | # || 8 | # pyr1 pyr0||oyr0 oyr1 9 | # ====================================================== 10 | init=dict() 11 | #======================================================= 12 | # Model and task for Observing Youbot (OY) 13 | # | oyr2 14 | # |oyr3 15 | # | 16 | # |oyr0 oyr1 17 | ################ OY motion ################## 18 | OY_node=[(-0.7,0.8,0.0),(-0.7,1.5,-1.57),(-0.2,1.5,-1.57),(-0.91,1.08,-0.79+1.57+0.3)] 19 | OY_label={ 20 | OY_node[0] : set(['oyr0']), 21 | OY_node[1]: set(['oyr1',]), 22 | OY_node[2]: set(['oyr2',]), 23 | OY_node[3]: set(['oyr3',]), 24 | } 25 | OY_init_pose=OY_node[0] 26 | OY_symbols=set(['oyr0','oyr1','oyr2','oyr3']) 27 | OY_motion=MotionFts(OY_label,OY_symbols,'OY-workspace') 28 | OY_motion.set_initial(OY_init_pose) 29 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 30 | OY_edge=[(OY_node[0],OY_node[1]),(OY_node[1],OY_node[2]),(OY_node[2],OY_node[3]), 31 | (OY_node[1],OY_node[3]),] 32 | OY_motion.add_un_edges(OY_edge,unit_cost=10.0) 33 | ########### OY action ########## 34 | OY_action_label={ 35 | 'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])), 36 | } 37 | OY_action = ActionModel(OY_action_label) 38 | ########### OY task ############ 39 | #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 40 | OY_task = '<>(oyr1 && <> (oyr2 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)' 41 | ########### OY initialize ############ 42 | init['OY']=(OY_motion, OY_action, OY_task) 43 | 44 | #======================================================= 45 | # Model and task for Pointing Youbot (PY) 46 | # pyr2 | 47 | # pyr3| 48 | # | 49 | # pyr1 pyr0| 50 | ########### PY motion ########## 51 | PY_node=[(-0.7,0.0,0.0),(-1.0,-1.0,0.0),(-0.0,-1.0,0.0),(-0.5,-0.8,-2.6+1.57)] 52 | PY_label={ 53 | PY_node[0]: set(['pyr0']), 54 | PY_node[1]: set(['pyr1',]), 55 | PY_node[2]: set(['pyr2',]), 56 | PY_node[3]: set(['pyr3',]), #'pyball' 57 | } 58 | PY_init_pose=PY_node[0] 59 | PY_symbols=set(['pyr0','pyr1','pyr2','pyr3']) 60 | PY_motion=MotionFts(PY_label,PY_symbols,'PY-workspace') 61 | PY_motion.set_initial(PY_init_pose) 62 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 63 | PY_edge=[(PY_node[0],PY_node[1]),(PY_node[1],PY_node[2]),(PY_node[2],PY_node[3]), 64 | (PY_node[1],PY_node[3]),] 65 | PY_motion.add_un_edges(PY_edge,unit_cost=10.0) 66 | ########### PY action ########## 67 | PY_action_label={ 68 | 'pypoint': (100, 'pyball', set(['pypoint'])), 69 | } 70 | PY_action = ActionModel(PY_action_label) 71 | ########### PY task ############ 72 | #PY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 73 | PY_task = '<>(pyr1 && (<> pyr2 && <> (pypoint))) && ([]<> pyr1) && ([]<> pyr2)' 74 | ########### PY initialize ############ 75 | init['PY']=(PY_motion, PY_action, PY_task) 76 | 77 | 78 | 79 | #======================================================= 80 | # Model and task for NAO (nao) 81 | # naor1 ||naor0 naor2 82 | ################ OY motion ################## 83 | NAO_node=[(-0.45,-1.1,0.0),(-0.0,-0.5,0.0),(-0.45,-1.1,0.0)] 84 | NAO_label={ 85 | NAO_node[0] : set(['naor0']), 86 | NAO_node[1]: set(['naor1',]), 87 | NAO_node[2]: set(['naor2','stopsign']), 88 | } 89 | NAO_init_pose=NAO_node[0] 90 | NAO_symbols=set(['naor0','naor1','naor2','stopsign']) 91 | NAO_motion=MotionFts(NAO_label,NAO_symbols,'NAO-workspace') 92 | NAO_motion.set_initial(NAO_init_pose) 93 | NAO_edge=[(NAO_node[0],NAO_node[1]),(NAO_node[1],NAO_node[2]),(NAO_node[0],NAO_node[2])] 94 | NAO_motion.add_un_edges(NAO_edge,unit_cost=10.0) 95 | ########### OY action ########## 96 | NAO_action_label={ 97 | 'naosit': (100, 'stopsign', set(['naosit',])), 98 | } 99 | NAO_action = ActionModel(NAO_action_label) 100 | ########### OY task ############ 101 | #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 102 | #NAO_task = '<>(naor1 && (<> naor0 && <> (naor2 && <> naosit))) && ([]<> naor1) && ([]<> naor2)' 103 | NAO_task = '<>(naor1 && (<> naor0 && <> (naor2))) && ([]<> naor1) && ([]<> naor2)' 104 | #NAO_task = '<>(naor1 && <> (naor2 && <> naosit)) && ([]<> naor1) && ([]<> naor2)' 105 | ########### OY initialize ############ 106 | init['NAO']=(NAO_motion, NAO_action, NAO_task) 107 | 108 | 109 | 110 | #============================================= 111 | # python adapt_planner_agent2.py PY 112 | # when manually send a message to PY 113 | # rostopic pub -1 knowledge_PY ltl3_NTUA/knowledge -- 'pyball' 'pyr3' 114 | # after PY updates plan, it moves to 'pyr3' to execute its action 'pypoint', 115 | # PY should publish message '['oyball', 'oyr3']' to topic 'knowledge_OY' 116 | # rostopic pub -1 knowledge_OY ltl3_NTUA/knowledge -- 'oyball' 'oyr3' 117 | # then OY would update its plan, and moves to 'oyr3' to execute its action 'oyobsgrasp' 118 | 119 | 120 | #========================================= 121 | # for testing 122 | # rostopic pub -1 activity_done_PY ltl3_NTUA/confirmation -- '0' 'goto' '10' 123 | # rostopic pub -1 activity_done_OY ltl3_NTUA/confirmation -- '0' 'goto' '10' 124 | -------------------------------------------------------------------------------- /scripts/reconfig/NTUA_init_2016_02.py: -------------------------------------------------------------------------------- 1 | from ltl_tools.ts import MotionFts, ActionModel 2 | #======================================================= 3 | # workspace layout for both agents 4 | # pyr2 || oyr2 5 | # pyr3||oyr3 6 | # || 7 | # pyr1 pyr0||oyr0 oyr1 8 | # ====================================================== 9 | init=dict() 10 | #======================================================= 11 | # Model and task for Observing Youbot (OY) 12 | # | oyr2 13 | # |oyr3 14 | # | 15 | # |oyr0 oyr1 16 | ################ OY motion ################## 17 | OY_node = [(-0.65,-1.0,0.0),(-0.65,-1.0,0.0),(0.4,-1.01,0.0),(-0.47,-1.1,-0.9)] 18 | #OY_node=[(-0.5,0.5,0.0),(-0.5,1.0,-1.57),(0.5,1.0,-1.57),(-0.0,0.5,-2.6)] 19 | #OY_node=[(-0.0,1.0,0.0),(-0.0,1.0,-0.0),(0.5,1.3,-1.57),(-0.00,0.5,-2.6)] 20 | OY_label={ 21 | OY_node[0] : set(['oyr0']), 22 | OY_node[1]: set(['oyr1',]), 23 | OY_node[2]: set(['oyr2',]), 24 | OY_node[3]: set(['oyr3',]), 25 | } 26 | OY_init_pose=OY_node[0] 27 | OY_symbols=set(['oyr0','oyr1','oyr2','oyr3']) 28 | OY_motion=MotionFts(OY_label,OY_symbols,'OY-workspace') 29 | OY_motion.set_initial(OY_init_pose) 30 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 31 | OY_edge=[(OY_node[0],OY_node[1]),(OY_node[1],OY_node[2]),(OY_node[2],OY_node[3]), 32 | (OY_node[1],OY_node[3]),] 33 | OY_motion.add_un_edges(OY_edge,unit_cost=10.0) 34 | ########### OY action ########## 35 | OY_action_label={ 36 | 'oyobsgrasp': (100, 'oyball', set(['oyobsgrasp'])), 37 | } 38 | OY_action = ActionModel(OY_action_label) 39 | ########### OY task ############ 40 | #OY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 41 | OY_task = '<>(oyr1 && <> (oyr2 && <> oyobsgrasp)) && ([]<> oyr1) && ([]<> oyr2)' 42 | ########### OY initialize ############ 43 | init['OY']=(OY_motion, OY_action, OY_task) 44 | 45 | #======================================================= 46 | # Model and task for Pointing Youbot (PY) 47 | # pyr2 | 48 | # pyr3| 49 | # | 50 | # pyr1 pyr0| 51 | ########### PY motion ########## 52 | PY_node= [(-0.62,1.0,0.0),(-0.4,0.8,0.0),(0.38,0.9,0.0),(-0.6,0.2,0.0)] 53 | #PY_node= [(-0.7,1.0,0.0),(-0.7,1.0,0.0),(0.38,0.9,0.0),(-0.1,0.5,0.0)] 54 | #PY_node=[(-0.8,-0.5,0.0),(-0.8,-1.3,1.57),(0.5,-1.3,1.57),(0.0,-0.5,-2.6)] 55 | PY_label={ 56 | PY_node[0]: set(['pyr0']), 57 | PY_node[1]: set(['pyr1',]), 58 | PY_node[2]: set(['pyr2',]), 59 | PY_node[3]: set(['pyr3',]), #'pyball' 60 | } 61 | PY_init_pose=PY_node[0] 62 | PY_symbols=set(['pyr0','pyr1','pyr2','pyr3']) 63 | PY_motion=MotionFts(PY_label,PY_symbols,'PY-workspace') 64 | PY_motion.set_initial(PY_init_pose) 65 | #B_edge=[(B_node[0],B_node[1]),(B_node[1],B_node[2]),(B_node[1],B_node[3]),(B_node[2],B_node[3]),(B_node[4],B_node[3]),(B_node[2],B_node[4])] 66 | PY_edge=[(PY_node[0],PY_node[1]),(PY_node[1],PY_node[2]),(PY_node[2],PY_node[3]), 67 | (PY_node[1],PY_node[3]),] 68 | PY_motion.add_un_edges(PY_edge,unit_cost=10.0) 69 | ########### PY action ########## 70 | PY_action_label={ 71 | 'pypoint': (100, 'pyball', set(['pypoint'])), 72 | } 73 | PY_action = ActionModel(PY_action_label) 74 | ########### PY task ############ 75 | #PY_task = '(<> grasp) && ([]<> r1) && ([]<> r2)' 76 | #PY_task = '<>(pyr1 && (<> pyr2 && <> (pyr3 && <> pypoint))) && ([]<> pyr1) && ([]<> pyr2)' 77 | ##PY_task = '<>(pyr1 && (<> pyr2 && <> (pypoint))) && ([]<> pyr1) && ([]<> pyr2)' 78 | PY_task = '<>(pyr1 && (<> pyr2 && <> (pypoint))) && ([]<> pyr1) && ([]<> pyr2) && ([]<> pyr3)' 79 | ########### PY initialize ############ 80 | init['PY']=(PY_motion, PY_action, PY_task) 81 | 82 | 83 | #============================================= 84 | # python adapt_planner_agent2.py PY 85 | # when manually send a message to PY 86 | # rostopic pub -1 knowledge_PY ltl3_NTUA/knowledge -- 'pyball' 'pyr3' 87 | # after PY updates plan, it moves to 'pyr3' to execute its action 'pypoint', 88 | # PY should publish message '['oyball', 'oyr3']' to topic 'knowledge_OY' 89 | # rostopic pub -1 knowledge_OY ltl3_NTUA/knowledge -- 'oyball' 'oyr3' 90 | # then OY would update its plan, and moves to 'oyr3' to execute its action 'oyobsgrasp' 91 | 92 | 93 | #========================================= 94 | # for testing 95 | # rostopic pub -1 activity_done_PY ltl3_NTUA/confirmation -- '0' 'goto' '10' 96 | # rostopic pub -1 activity_done_OY ltl3_NTUA/confirmation -- '0' 'goto' '10' 97 | # rostopic echo /next_activity_PY 98 | # rostopic pub -1 activity_done_PY ltl3_NTUA/confirmation -- '2' 'pypoint' '10' 99 | # rostopic pub -1 activity_done_OY ltl3_NTUA/confirmation -- '3' 'oyobsgrasp' '10' 100 | -------------------------------------------------------------------------------- /scripts/reconfig/adapt_planner_agent.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | import numpy 4 | import Queue 5 | roslib.load_manifest('ltl3') 6 | import rospy 7 | from ltl3.msg import pose, activity, confirmation, knowledge 8 | from math import sqrt, cos, sin, radians 9 | import numpy 10 | from NTUA_init import * 11 | import sys 12 | 13 | from ltl_tools.ts import MotionFts, ActionModel, MotActModel 14 | from ltl_tools.planner import ltl_planner 15 | 16 | 17 | confirm = ['none', 0] 18 | object_name = None 19 | region = None 20 | 21 | def distance(pose1, pose2): 22 | return sqrt( (pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2 ) 23 | 24 | def confirm_callback(data): 25 | global confirm 26 | name = data.name 27 | done = data.done 28 | confirm = [name, done] 29 | print "confirm", confirm 30 | 31 | def knowledge_callback(data): 32 | global object_name 33 | global region 34 | object_name = data.object 35 | region = data.region 36 | print 'object [', object_name, '] detected at', region 37 | 38 | 39 | def planner(letter, ts, act, task): 40 | global POSE 41 | global c 42 | global confirm 43 | global object_name 44 | global region 45 | rospy.init_node('planner_%s' %letter) 46 | print 'Agent %s: planner started!' %(letter) 47 | ###### publish to 48 | activity_pub = rospy.Publisher('next_move_%s' %letter, activity, queue_size=10) 49 | ###### subscribe to 50 | rospy.Subscriber('activity_done_%s' %letter, confirmation, confirm_callback) 51 | rospy.Subscriber('knowledge_%s' %letter, knowledge, knowledge_callback) 52 | ####### agent information 53 | c = 0 54 | k = 0 55 | flag = 0 56 | full_model = MotActModel(ts, act) 57 | #planner = ltl_planner(full_model, task, None) 58 | planner = ltl_planner(full_model, task, None) 59 | ####### initial plan synthesis 60 | planner.optimal(10) 61 | ####### 62 | while not rospy.is_shutdown(): 63 | next_activity = activity() 64 | ############### check for knowledge udpate 65 | if object_name: 66 | # konwledge detected 67 | planner.update(object_name, region) 68 | print 'Agent %s: object incorporated in map!' %(letter,) 69 | planner.replan_simple() 70 | object_name = None 71 | ############### send next move 72 | next_move = planner.next_move 73 | next_state = planner.next_state 74 | ############### implement next activity 75 | if isinstance(next_move, str): 76 | # next activity is action 77 | next_activity.type = next_move 78 | next_activity.x = 0 79 | next_activity.y = 0 80 | print 'Agent %s: next action %s!' %(letter, next_activity.type) 81 | while not ((confirm[0]==next_move) and (confirm[1]>0)): 82 | activity_pub.publish(next_activity) 83 | rospy.sleep(0.06) 84 | rospy.sleep(1) 85 | confirm[1] = 0 86 | print 'Agent %s: action %s done!' %(letter, next_activity.type) 87 | else: 88 | print 'Agent %s: next waypoint (%.2f,%.2f)!' %(letter, next_move[0], next_move[1]) 89 | while not ((confirm[0]=='goto') and (confirm[1]>0)): 90 | #relative_x = next_move[0]-POSE[0] 91 | #relative_y = next_move[1]-POSE[1] 92 | #relative_pose = [relative_x, relative_y] 93 | #oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2]) 94 | next_activity.type = 'goto' 95 | #next_activity.x = oriented_relative_pose[0] 96 | #next_activity.y = oriented_relative_pose[1] 97 | next_activity.x = next_move[0] 98 | next_activity.y = next_move[1] 99 | activity_pub.publish(next_activity) 100 | rospy.sleep(0.06) 101 | rospy.sleep(1) 102 | confirm[1] = 0 103 | print 'Agent %s: waypoint (%.2f,%.2f) reached!' %(letter, next_move[0], next_move[1]) 104 | planner.pose = [next_move[0], next_move[1]] 105 | planner.find_next_move() 106 | 107 | def planner_agent(agent_letter): 108 | if agent_letter in init: 109 | agent_ts, agent_act, agent_task = init[agent_letter] 110 | planner(agent_letter, agent_ts, agent_act, agent_task) 111 | else: 112 | print('Agent not specified in init.py') 113 | 114 | 115 | if __name__ == '__main__': 116 | ######## 117 | if len(sys.argv) == 2: 118 | agent_letter = str(sys.argv[1]) 119 | # to run: python planner_agent.py B 120 | ############### 121 | try: 122 | planner_agent(agent_letter) 123 | except rospy.ROSInterruptException: 124 | pass 125 | -------------------------------------------------------------------------------- /scripts/reconfig/adapt_planner_agent2.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | import numpy 4 | import Queue 5 | roslib.load_manifest('ltl3_NTUA') 6 | import rospy 7 | from ltl3_NTUA.msg import pose, activity, confirmation, knowledge 8 | from math import sqrt, cos, sin, radians 9 | import numpy 10 | from NTUA_init_2016_02 import * 11 | #from test import * 12 | import sys 13 | 14 | from ltl_tools.ts import MotionFts, ActionModel, MotActModel 15 | from ltl_tools.planner import ltl_planner 16 | 17 | 18 | confirm = ['none', 0, 0] 19 | object_name = None 20 | region = None 21 | header = 0 22 | confheader = 0 23 | 24 | def distance(pose1, pose2): 25 | return sqrt( (pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2 ) 26 | 27 | def confirm_callback(data): 28 | global confirm 29 | global confheader 30 | 31 | name = data.name 32 | done = data.done 33 | confheader = data.confheader 34 | 35 | confirm = [name, done, confheader] 36 | print "confirm", confirm 37 | 38 | def knowledge_callback(data): 39 | global object_name 40 | global region 41 | object_name = data.object 42 | region = data.region 43 | print 'object [', object_name, '] detected at', region 44 | 45 | 46 | def planner(letter, ts, act, task): 47 | global header 48 | global POSE 49 | global c 50 | global confirm 51 | global object_name 52 | global region 53 | rospy.init_node('planner_%s' %letter) 54 | print 'Agent %s: planner started!' %(letter) 55 | ###### publish to 56 | activity_pub = rospy.Publisher('next_move_%s' %letter, activity, queue_size=10) 57 | ###### subscribe to 58 | rospy.Subscriber('activity_done_%s' %letter, confirmation, confirm_callback) 59 | rospy.Subscriber('knowledge_%s' %letter, knowledge, knowledge_callback) 60 | 61 | ####### agent information 62 | c = 0 63 | k = 0 64 | flag = 0 65 | full_model = MotActModel(ts, act) 66 | planner = ltl_planner(full_model, None, task) 67 | #planner = ltl_planner(full_model, task, None) 68 | ####### initial plan synthesis 69 | planner.optimal(10) 70 | ####### 71 | while not rospy.is_shutdown(): 72 | next_activity = activity() 73 | ############### check for knowledge udpate 74 | if object_name: 75 | # konwledge detected 76 | planner.update(object_name, region) 77 | print 'Agent %s: object incorporated in map!' %(letter,) 78 | planner.replan_simple() 79 | object_name = None 80 | ############### send next move 81 | next_move = planner.next_move 82 | next_state = planner.next_state 83 | ############### implement next activity 84 | if isinstance(next_move, str): 85 | # next activity is action 86 | next_activity.header = header 87 | next_activity.type = next_move 88 | next_activity.x = -0.76 89 | next_activity.y = 0.30 90 | print 'Agent %s: next action %s!' %(letter, next_activity.type) 91 | #while not ((confirm[0]==next_move) and (confirm[1]>0) and confirm[2] == header): 92 | while (not ((confirm[0]==next_move) and (confirm[1]>0) ) and not rospy.is_shutdown() ): 93 | activity_pub.publish(next_activity) 94 | rospy.sleep(0.06) 95 | rospy.sleep(1) 96 | confirm[1] = 0 97 | header = header + 1 98 | print 'Agent %s: action %s done!' %(letter, next_activity.type) 99 | else: 100 | print 'Agent %s: next waypoint (%.2f,%.2f,%.2f)!' %(letter, next_move[0], next_move[1], next_move[2]) 101 | while ((not ((confirm[0]=='goto') and (confirm[1]>0) and confirm[2] == header) ) and not rospy.is_shutdown() ): 102 | #relative_x = next_move[0]-POSE[0] 103 | #relative_y = next_move[1]-POSE[1] 104 | #relative_pose = [relative_x, relative_y] 105 | #oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2]) 106 | next_activity.type = 'goto' 107 | next_activity.header = header 108 | #next_activity.x = oriented_relative_pose[0] 109 | #next_activity.y = oriented_relative_pose[1] 110 | next_activity.x = next_move[0] 111 | next_activity.y = next_move[1] 112 | next_activity.psi = next_move[2] 113 | activity_pub.publish(next_activity) 114 | rospy.sleep(0.06) 115 | rospy.sleep(1) 116 | confirm[1] = 0 117 | header = header + 1 118 | print 'Agent %s: waypoint (%.2f,%.2f,%.2f) reached!' %(letter, next_move[0], next_move[1], next_move[2]) 119 | planner.pose = [next_move[0], next_move[1]] 120 | planner.find_next_move() 121 | 122 | def planner_agent(agent_letter): 123 | if agent_letter in init: 124 | agent_ts, agent_act, agent_task = init[agent_letter] 125 | planner(agent_letter, agent_ts, agent_act, agent_task) 126 | else: 127 | print('Agent not specified in init.py') 128 | 129 | 130 | if __name__ == '__main__': 131 | ######## 132 | if len(sys.argv) == 2: 133 | agent_letter = str(sys.argv[1]) 134 | # to run: python planner_agent.py B 135 | ############### 136 | try: 137 | planner_agent(agent_letter) 138 | except rospy.ROSInterruptException: 139 | print 'abc' 140 | pass 141 | -------------------------------------------------------------------------------- /scripts/reconfig/init.py: -------------------------------------------------------------------------------- 1 | 2 | from ltl_tools.ts import MotionFts, ActionModel 3 | 4 | # # to trun python planner_agent.py B 5 | # specify the agent plans here 6 | init=dict() 7 | ################### 8 | ####### workspace dimension 9 | # 175 10 | # 105 11 | # 35 12 | # 40 120 200 13 | # x: 40, 120, 200 14 | # y: 35, 105, 175 15 | ##################################################### 16 | 17 | #======================================================= 18 | #======================================================= 19 | #======================================================= 20 | ########### B motion ########## 21 | colormap = {'ball':'red', 'obs':'black','basket':'yellow'} 22 | B_symbols = colormap.keys() 23 | WIDTH = 240 # cm 24 | HEIGHT = 210 # cm 25 | N = 6.0 26 | RATE = 1/N 27 | B_init_pose = (WIDTH*RATE,HEIGHT*RATE); 28 | B_node_dict ={ 29 | # lower three rooms 30 | (WIDTH*RATE,HEIGHT*RATE): set(['r1']), 31 | (WIDTH*3*RATE,HEIGHT*RATE): set(['r2']), 32 | (WIDTH*5*RATE,HEIGHT*RATE): set(['r3']), 33 | # cooridor three parts 34 | (WIDTH*RATE,HEIGHT*3*RATE): set(['c1','ball']), 35 | (WIDTH*3*RATE,HEIGHT*3*RATE): set(['c2',]), 36 | (WIDTH*5*RATE,HEIGHT*3*RATE): set(['c3']), 37 | # upper three rooms 38 | (WIDTH*RATE,HEIGHT*5*RATE): set(['r4',]), 39 | (WIDTH*3*RATE,HEIGHT*5*RATE): set(['r5','basket']), 40 | (WIDTH*5*RATE,HEIGHT*5*RATE): set(['r6']), 41 | } 42 | B_motion = MotionFts(B_node_dict, B_symbols, 'office') 43 | B_motion.set_initial(B_init_pose) 44 | B_edge_list = [ # 1st column 45 | ((WIDTH*RATE,HEIGHT*RATE), (WIDTH*RATE,HEIGHT*3*RATE)), 46 | ((WIDTH*RATE,HEIGHT*3*RATE), (WIDTH*RATE,HEIGHT*5*RATE)), 47 | # 2nd row 48 | ((WIDTH*RATE,HEIGHT*3*RATE), (WIDTH*3*RATE,HEIGHT*3*RATE)), 49 | ((WIDTH*3*RATE,HEIGHT*3*RATE), (WIDTH*5*RATE,HEIGHT*3*RATE)), 50 | # 2nd column 51 | ((WIDTH*3*RATE,HEIGHT*RATE), (WIDTH*3*RATE,HEIGHT*3*RATE)), 52 | ((WIDTH*3*RATE,HEIGHT*3*RATE), (WIDTH*3*RATE,HEIGHT*5*RATE)), 53 | # 3rd column 54 | ((WIDTH*5*RATE,HEIGHT*RATE), (WIDTH*5*RATE,HEIGHT*3*RATE)), 55 | ((WIDTH*5*RATE,HEIGHT*3*RATE), (WIDTH*5*RATE,HEIGHT*5*RATE)), 56 | ] 57 | B_motion.add_un_edges(B_edge_list,unit_cost=0.1) 58 | ############################### 59 | ########### B action ########## 60 | B_action_dict={ 61 | 'grasp': (100, 'ball', set(['grasp'])), 62 | 'throw': (60, 'basket', set(['throw'])) 63 | } 64 | B_action = ActionModel(B_action_dict) 65 | ############################### 66 | ########### B task ############ 67 | B_task = '(<> (grasp && <> throw)) && (<>[] r1)' 68 | #B_task = '<> grasp && (<>[] r2)' 69 | ####################### 70 | init['B']=(B_motion, B_action, B_task) 71 | 72 | ##################################################### 73 | #======================================================= 74 | #======================================================= 75 | #======================================================= 76 | ########### C motion ########## 77 | colormap = {'ball':'red', 'obs':'black','basket':'yellow'} 78 | C_symbols = colormap.keys() 79 | WIDTH = 240 # cm 80 | HEIGHT = 210 # cm 81 | N = 6.0 82 | RATE = 1/N 83 | C_init_pose = (WIDTH*5*RATE,HEIGHT*1*RATE); 84 | C_node_dict ={ 85 | # lower three rooms 86 | (WIDTH*RATE,HEIGHT*RATE): set(['r1']), 87 | (WIDTH*3*RATE,HEIGHT*RATE): set(['r2']), 88 | (WIDTH*5*RATE,HEIGHT*RATE): set(['r3']), 89 | # cooridor three parts 90 | (WIDTH*RATE,HEIGHT*3*RATE): set(['c1']), 91 | (WIDTH*3*RATE,HEIGHT*3*RATE): set(['c2']), 92 | (WIDTH*5*RATE,HEIGHT*3*RATE): set(['c3','cushion']), 93 | # upper three rooms 94 | (WIDTH*RATE,HEIGHT*5*RATE): set(['r4']), 95 | (WIDTH*3*RATE,HEIGHT*5*RATE): set(['r5','basket']), 96 | (WIDTH*5*RATE,HEIGHT*5*RATE): set(['r6']), 97 | } 98 | C_motion = MotionFts(C_node_dict, C_symbols, 'office') 99 | C_motion.set_initial(C_init_pose) 100 | edge_list = [ # 1st column 101 | ((WIDTH*RATE,HEIGHT*RATE), (WIDTH*RATE,HEIGHT*3*RATE)), 102 | ((WIDTH*RATE,HEIGHT*3*RATE), (WIDTH*RATE,HEIGHT*5*RATE)), 103 | # 2nd row 104 | ((WIDTH*RATE,HEIGHT*3*RATE), (WIDTH*3*RATE,HEIGHT*3*RATE)), 105 | ((WIDTH*3*RATE,HEIGHT*3*RATE), (WIDTH*5*RATE,HEIGHT*3*RATE)), 106 | # 2nd column 107 | ((WIDTH*3*RATE,HEIGHT*RATE), (WIDTH*3*RATE,HEIGHT*3*RATE)), 108 | ((WIDTH*3*RATE,HEIGHT*3*RATE), (WIDTH*3*RATE,HEIGHT*5*RATE)), 109 | # 3rd column 110 | ((WIDTH*5*RATE,HEIGHT*RATE), (WIDTH*5*RATE,HEIGHT*3*RATE)), 111 | ((WIDTH*5*RATE,HEIGHT*3*RATE), (WIDTH*5*RATE,HEIGHT*5*RATE)), 112 | ] 113 | C_motion.add_un_edges(edge_list,unit_cost=0.1) 114 | ############################### 115 | ########### B action ########## 116 | C_action_dict={ 117 | 'grasp': (100, 'ball', set(['grasp'])), 118 | 'throw': (60, 'basket', set(['throw'])), 119 | 'crouch': (60, 'cushion', set(['crouch'])), 120 | } 121 | C_action = ActionModel(C_action_dict) 122 | ############################### 123 | ########### B task ############ 124 | C_task = '(<> (crouch && <> (throw))) && (<>[] r3)' 125 | ####################### 126 | init['C']=(C_motion, C_action, C_task) 127 | ##################################################### 128 | -------------------------------------------------------------------------------- /scripts/reconfig/nao_static.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | import numpy 4 | import Queue 5 | #roslib.load_manifest('ltl3') 6 | import rospy 7 | from ltl3_NTUA.msg import pose, confirmation, knowledge 8 | from math import sqrt, cos, sin, radians 9 | import numpy 10 | #from NTUA_init import * 11 | import sys 12 | from nao_basic.msg import * 13 | from ar_pose.msg import * 14 | #from ltl_tools.ts import MotionFts, ActionModel, MotActModel 15 | #from ltl_tools.planner import ltl_planner 16 | from geometry_msgs.msg import Quaternion, Vector3 17 | from math import floor, exp, sqrt, atan2, sin, pi, asin, cos 18 | 19 | ac_POSE = Queue.Queue(5) 20 | POSE = [0.0, 0.0, 0.0] 21 | 22 | confirm = ['none', 0] 23 | object_name = None 24 | region = None 25 | 26 | def distance(pose1, pose2): 27 | return sqrt( (pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2 ) 28 | 29 | def pose_callback(m): # data = '(x, y, theta)' x, y in cm, theta in radian 30 | 31 | global POSE 32 | for poses in m.markers: 33 | if poses.id == 2: 34 | #print "updating pose 2" 35 | POSE[0]= poses.pose.pose.position.x 36 | POSE[1] = -poses.pose.pose.position.y 37 | 38 | quat2 = Quaternion(0,0,poses.pose.pose.orientation.y,poses.pose.pose.orientation.x) 39 | euler2 = quat2eulerZYX (quat2) 40 | POSE[2] = -euler2.z 41 | 42 | 43 | #Quaternion to Euler-ZYX 44 | def quat2eulerZYX (q): 45 | euler = Vector3() 46 | tol = quat2eulerZYX.tolerance 47 | 48 | qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z 49 | qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z 50 | qwy, qwz = q.w*q.y, q.w*q.z 51 | 52 | test = -2.0 * (qxz - qwy) 53 | if test > +tol: 54 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 55 | euler.y = +0.5 * pi 56 | euler.z = 0.0 57 | 58 | return euler 59 | 60 | elif test < -tol: 61 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 62 | euler.y = -0.5 * pi 63 | euler.z = tol 64 | 65 | return euler 66 | 67 | else: 68 | euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz) 69 | euler.y = asin (test) 70 | euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz) 71 | 72 | return euler 73 | quat2eulerZYX.tolerance=0.99999 74 | 75 | 76 | def check_dist(cur_pose, waypoint): 77 | dist = distance(cur_pose, waypoint) 78 | # print "dist", dist 79 | if (dist <= 0.5): 80 | # print "**********reached waypoint**********" 81 | return True 82 | return False 83 | 84 | def rotate_2d_vector(v, theta): 85 | new_v = [0,0] 86 | new_v[0] = cos(theta)*v[0] - sin(theta)*v[1] 87 | new_v[1] = sin(theta)*v[0] + cos(theta)*v[1] 88 | return new_v 89 | 90 | def planner( ): 91 | global POSE 92 | letter = 'NAO' 93 | rospy.init_node('planner_%s' %letter) 94 | print 'Agent %s: planner started!' %(letter) 95 | ###### publish to 96 | activity_pub = rospy.Publisher('next_move_mover', activity, queue_size=10) 97 | ###### subscribe to 98 | rospy.Subscriber("/ceiling/pose", ARMarkersExtended, pose_callback) 99 | ############################## 100 | #wps = ((0.08, y) for y in [1.97-0.5*k for k in xrange(0,7)]) 101 | #Waypoints = [a for a in wps] 102 | Waypoints = [[0.64, -0.48],[0.41,0.7]] 103 | next_activity = activity() 104 | print 'NAO waypoints:' 105 | print Waypoints 106 | ################## 107 | ############### implement next activity 108 | direction = 1 #1 from door to pool, -1 from pool to door 109 | K = 0 110 | while True: 111 | while ((K < len(Waypoints) -1 and direction == 1) or (K > 0 and direction == -1)): 112 | wp = Waypoints[K] 113 | print 'Agent %s: next waypoint (%.2f,%.2f)!' %(letter, wp[0], wp[1]) 114 | while not (check_dist(POSE[0:2], wp)): 115 | relative_x = wp[0]-POSE[0] 116 | relative_y = wp[1]-POSE[1] 117 | relative_pose = [relative_x, relative_y] 118 | oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2]) 119 | next_activity.type = 'goto' 120 | next_activity.x = oriented_relative_pose[0]*100 121 | next_activity.y = oriented_relative_pose[1]*100 122 | activity_pub.publish(next_activity) 123 | rospy.sleep(1) 124 | print 'Agent %s: waypoint (%.2f,%.2f) reached!' %(letter, wp[0], wp[1]) 125 | K += 1*direction 126 | print 'ALL waypoints finished' 127 | direction*= -1 128 | 129 | 130 | if __name__ == '__main__': 131 | ######## 132 | if len(sys.argv) == 2: 133 | agent_letter = str(sys.argv[1]) 134 | # to run: python planner_agent.py B 135 | ############### 136 | try: 137 | planner() 138 | except rospy.ROSInterruptException: 139 | pass 140 | -------------------------------------------------------------------------------- /scripts/reconfig/nao_walker.py: -------------------------------------------------------------------------------- 1 | # -*- encoding: UTF-8 -*- 2 | 3 | '''Move To: Small example to make Nao Move To an Objective''' 4 | 5 | import math 6 | import sys 7 | from naoqi import ALProxy 8 | 9 | import roslib 10 | import numpy 11 | import Queue 12 | roslib.load_manifest('ltl3_NTUA') 13 | import rospy 14 | from ltl3_NTUA.msg import pose, activity, confirmation, knowledge 15 | 16 | goal_x = 99.0; 17 | goal_y = 99.0; 18 | goal_t = 3.0; 19 | 20 | def StiffnessOn(proxy): 21 | pNames = "Body" 22 | pStiffnessLists = 1.0 23 | pTimeLists = 1.0 24 | proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists) 25 | 26 | 27 | def StiffnessOff(proxy): 28 | pNames = "Body" 29 | pStiffnessLists = 0.0 30 | pTimeLists = 1.0 31 | proxy.stiffnessInterpolation(pNames, pStiffnessLists, pTimeLists) 32 | 33 | def new_waypoint_callback(data): 34 | global now 35 | now = rospy.get_rostime() 36 | global goal_x; 37 | global goal_y; 38 | global goal_t; 39 | goal_x = 0.01*data.x; 40 | goal_y = 0.01*data.y; 41 | goal_t = -data.psi; 42 | print "goal", goal_x, goal_y, goal_t; 43 | 44 | 45 | def main(robotIP): 46 | try: 47 | motionProxy = ALProxy("ALMotion", robotIP, 9559) 48 | except Exception, e: 49 | print "Could not create proxy to ALMotion" 50 | print "Error was: ", e 51 | print "Try running the script again" 52 | 53 | 54 | rospy.Subscriber('next_move_PY_relative', activity, new_waypoint_callback) 55 | 56 | StiffnessOn(motionProxy) 57 | motionProxy.setWalkArmsEnabled(True, True) 58 | motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION", True]]) 59 | 60 | #~ motionProxy.setWalkArmsEnabled(False, False) 61 | #~ motionProxy.setMotionConfig([["ENABLE_FOOT_CONTACT_PROTECTION",False]]) 62 | # motionProxy.waitUntilMoveIsFinished() 63 | global goal_x, goal_y, goal_t; 64 | 65 | while not rospy.is_shutdown(): 66 | if (rospy.get_rostime() - now < rospy.Duration(0.2) ): 67 | Theta = numpy.arctan2(goal_y, goal_x) 68 | if ( abs(goal_x) < 0.20 and abs(goal_y) < 0.20 and abs(goal_t) < 0.4): 69 | print "************************************************************Location reached, waiting for the next waypoint" 70 | continue 71 | if ( abs(goal_x) < 0.20 and abs(goal_y) < 0.20 ): 72 | print '************************************************************case 1: turning towards final orientation' 73 | motionProxy.post.moveTo(0.0, 0.0, goal_t, 74 | [ ["MaxStepX", 0.02], # step of 2 cm in front 75 | ["MaxStepY", 0.16], # default value 76 | ["MaxStepTheta", 0.2], # default value 77 | ["MaxStepFrequency", 0.0], # low frequency 78 | ["StepHeight", 0.01], # step height of 1 cm 79 | ["TorsoWx", 0.0], # default value 80 | ["TorsoWy", 0.1] ]) # torso bend 0.1 rad in front 81 | elif ( abs(goal_y) < 0.30 ): 82 | print '************************************************************case 2: moving forward' 83 | motionProxy.post.moveTo(goal_x, goal_y, Theta, 84 | [ ["MaxStepX", 0.02], # step of 2 cm in front 85 | ["MaxStepY", 0.16], # default value 86 | ["MaxStepTheta", 0.2], # default value 87 | ["MaxStepFrequency", 0.0], # low frequency 88 | ["StepHeight", 0.01], # step height of 1 cm 89 | ["TorsoWx", 0.0], # default value 90 | ["TorsoWy", 0.1] ]) # torso bend 0.1 rad in front 91 | else: 92 | print '************************************************************case 3: rotating around' 93 | motionProxy.post.moveTo(0.0, 0.0, Theta, 94 | [ ["MaxStepX", 0.02], # step of 2 cm in front 95 | ["MaxStepY", 0.16], # default value 96 | ["MaxStepTheta", 0.2], # default value 97 | ["MaxStepFrequency", 0.0], # low frequency 98 | ["StepHeight", 0.01], # step height of 1 cm 99 | ["TorsoWx", 0.0], # default value 100 | ["TorsoWy", 0.1] ]) # torso bend 0.1 rad in front 101 | rospy.sleep(0.5) 102 | else: 103 | print "waypoint too old" 104 | StiffnessOff(motionProxy) 105 | 106 | 107 | if __name__ == "__main__": 108 | robotIp = "192.168.2.197" 109 | 110 | global now 111 | rospy.init_node('nao_wp_tracker') 112 | now = rospy.get_rostime() 113 | 114 | if len(sys.argv) <= 1: 115 | print "Usage python motion_moveTo.py robotIP (optional default: 127.0.0.1)" 116 | else: 117 | robotIp = sys.argv[1] 118 | 119 | main(robotIp) 120 | 121 | -------------------------------------------------------------------------------- /scripts/reconfig/observeGrasp_OY.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib 5 | import rospy 6 | import time 7 | # Msgs imports 8 | from nav_msgs.msg import Odometry 9 | from geometry_msgs.msg import Twist 10 | import std_msgs 11 | 12 | # Service imports 13 | from std_srvs.srv import Empty, EmptyResponse 14 | 15 | # Python imports 16 | import numpy as np 17 | from numpy.linalg import * 18 | import math as math1 19 | 20 | #Custom imports 21 | #from eulerangles import * 22 | import sys 23 | from math import floor, exp, sqrt, atan2, sin, pi, asin, cos 24 | from geometry_msgs.msg import Point, Vector3, Quaternion 25 | from math_utils import * 26 | 27 | from poseArmController import * 28 | 29 | 30 | #planner messages 31 | from ltl3_NTUA.msg import * 32 | 33 | 34 | 35 | enable = False 36 | header = -1 37 | 38 | def enableSrv(req): 39 | global enable 40 | enable = True 41 | rospy.loginfo('%s Enabled', 'Gesture') 42 | return EmptyResponse() 43 | 44 | 45 | def disableSrv(req): 46 | global enable 47 | enable = False 48 | rospy.loginfo('%s Disabled', 'Gesture') 49 | return EmptyResponse() 50 | 51 | flag_conf = True 52 | 53 | def updateNextMove(data): 54 | Action = data.type 55 | global header, flag_conf, enable 56 | 57 | if data.header != header: 58 | flag_conf = True 59 | header = data.header 60 | 61 | if data.type == 'oyobsgrasp' and flag_conf == True: 62 | print "recognizing gesture" 63 | time.sleep(10) 64 | pub_observingdone.publish(1) 65 | print "I published the gesture was recognized" 66 | enable = True 67 | print "grasping object" 68 | time.sleep(5) 69 | enable = False 70 | flag_conf = False 71 | confMsg = confirmation() 72 | confMsg.name = 'oyobsgrasp' 73 | confMsg.done = 1 74 | confMsg.confheader = header 75 | print confMsg.done 76 | pub_confirm.publish(confMsg) 77 | print "I published the confirmation of oyobsgrasp" 78 | 79 | 80 | rospy.Subscriber("/next_move_OY", activity, updateNextMove) 81 | pub_confirm = rospy.Publisher('activity_done_OY', confirmation) 82 | 83 | pub_observingdone = rospy.Publisher('/OY/observation_done', std_msgs.msg.Int8) 84 | 85 | 86 | counter = 0; 87 | if __name__ == '__main__': 88 | try: 89 | rospy.init_node('observe_node') 90 | #Arm Controller Object 91 | rospy.sleep(1.0/300.0) 92 | rospy.spin() 93 | except rospy.ROSInterruptException: pass 94 | -------------------------------------------------------------------------------- /scripts/reconfig/planner_agent.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | import numpy 4 | import Queue 5 | roslib.load_manifest('ltl3_NTUA') 6 | import rospy 7 | from ltl3.msg import pose, activity, confirmation, knowledge 8 | from math import sqrt, cos, sin, radians 9 | import numpy 10 | from init import * 11 | import sys 12 | 13 | from ltl_tools.ts import MotionFts, ActionModel, MotActModel 14 | from ltl_tools.planner import ltl_planner 15 | 16 | ac_POSE = Queue.Queue(5) 17 | POSE = [0, 0, 0] 18 | 19 | confirm = ['none', 0] 20 | object_name = None 21 | 22 | def distance(pose1, pose2): 23 | return sqrt( (pose1[0]-pose2[0])**2+(pose1[1]-pose2[1])**2 ) 24 | 25 | def pose_callback(data): # data = '(x, y, theta)' x, y in cm, theta in radians 26 | global ac_POSE 27 | global POSE 28 | while ac_POSE.qsize() < 5: 29 | # print "while loop" 30 | ac_POSE.put([data.x, data.y, data.theta]) 31 | # print "inserting to FIFO queue", [data.x, data.y, data.theta] 32 | else: 33 | #print "else" 34 | ac_POSE.get() 35 | ac_POSE.put([data.x, data.y, data.theta]) 36 | # print "inserting to FIFO queue", [data.x, data.y, data.theta] 37 | # print "qsize", ac_POSE.qsize() 38 | POSE = median_filter(ac_POSE) 39 | 40 | def confirm_callback(data): 41 | global confirm 42 | name = data.name 43 | done = data.done 44 | confirm = [name, done] 45 | print "confirm", confirm 46 | 47 | def knowledge_callback(data): 48 | global object_name 49 | object_name = data.object 50 | print 'object [', object_name, '] detected!' 51 | 52 | def median_filter(raw_pose): 53 | list_pose = list(raw_pose.queue) 54 | x_list = [pose[0] for pose in list_pose] 55 | median_x = numpy.median(x_list) 56 | y_list = [pose[1] for pose in list_pose] 57 | median_y = numpy.median(y_list) 58 | theta_list = [pose[2] for pose in list_pose] 59 | median_theta = numpy.median(theta_list) 60 | return [median_x, median_y, median_theta] 61 | 62 | def check_dist(cur_pose, waypoint): 63 | dist = distance(cur_pose, waypoint) 64 | # print "dist", dist 65 | if (dist <= 30.0): 66 | # print "**********reached waypoint**********" 67 | return True 68 | return False 69 | 70 | def rotate_2d_vector(v, theta): 71 | new_v = [0,0] 72 | new_v[0] = cos(theta)*v[0] - sin(theta)*v[1] 73 | new_v[1] = sin(theta)*v[0] + cos(theta)*v[1] 74 | return new_v 75 | 76 | def planner(letter, ts, act, task): 77 | global POSE 78 | global c 79 | global confirm 80 | rospy.init_node('planner_%s' %letter) 81 | print 'Agent %s: planner started!' %(letter) 82 | ###### publish to 83 | activity_pub = rospy.Publisher('next_move_%s' %letter, activity) 84 | ###### subscribe to 85 | rospy.Subscriber('cur_pose_%s' %letter, pose, pose_callback) 86 | rospy.Subscriber('activity_done_%s' %letter, confirmation, confirm_callback) 87 | rospy.Subscriber('knowledge_%s' %letter, knowledge, knowledge_callback) 88 | ####### agent information 89 | c = 0 90 | k = 0 91 | flag = 0 92 | full_model = MotActModel(ts, act) 93 | planner = ltl_planner(full_model, task, None) 94 | ####### initial plan synthesis 95 | planner.optimal(10,'static') 96 | ####### 97 | while not rospy.is_shutdown(): 98 | while not POSE: 99 | rospy.sleep(0.1) 100 | planner.cur_pose = POSE[0:2] 101 | next_activity_bowser = activity() 102 | next_activity = activity() 103 | ############### check for knowledge udpate 104 | if object_name: 105 | # konwledge detected 106 | planner.update(object_name) 107 | print 'Agent %s: object incorporated in map!' %(letter,) 108 | planner.replan() 109 | ############### send next move 110 | next_move = planner.next_move 111 | ############### implement next activity 112 | if isinstance(next_move, str): 113 | # next activity is action 114 | next_activity.type = next_move 115 | next_activity.x = 0 116 | next_activity.y = 0 117 | print 'Agent %s: next action %s!' %(letter, next_activity.type) 118 | while not ((confirm[0]==next_move) and (confirm[1]>0)): 119 | activity_pub.publish(next_activity) 120 | rospy.sleep(0.5) 121 | print 'Agent %s: action %s done!' %(letter, next_activity.type) 122 | else: 123 | print 'Agent %s: next waypoint (%d,%d)!' %(letter, next_move[0], next_move[1]) 124 | while not (check_dist(POSE[0:2], next_move)): 125 | relative_x = next_move[0]-POSE[0] 126 | relative_y = next_move[1]-POSE[1] 127 | relative_pose = [relative_x, relative_y] 128 | oriented_relative_pose = rotate_2d_vector(relative_pose, -POSE[2]) 129 | next_activity.type = 'goto' 130 | next_activity.x = oriented_relative_pose[0] 131 | next_activity.y = oriented_relative_pose[1] 132 | activity_pub.publish(next_activity) 133 | rospy.sleep(0.5) 134 | print 'Agent %s: waypoint (%d,%d) reached!' %(letter, next_move[0], next_move[1]) 135 | planner.trace.append(planner.find_next_move()) 136 | 137 | def planner_agent(agent_letter): 138 | if agent_letter in init: 139 | agent_ts, agent_act, agent_task = init[agent_letter] 140 | planner(agent_letter, agent_ts, agent_act, agent_task) 141 | else: 142 | print('Agent not specified in init.py') 143 | 144 | 145 | if __name__ == '__main__': 146 | ######## 147 | if len(sys.argv) == 2: 148 | agent_letter = str(sys.argv[1]) 149 | # to run: python planner_agent.py B 150 | ############### 151 | try: 152 | planner_agent(agent_letter) 153 | except rospy.ROSInterruptException: 154 | pass 155 | -------------------------------------------------------------------------------- /scripts/reconfig/poseArmController.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib 5 | import rospy 6 | 7 | # Msgs imports 8 | from brics_actuator.msg import JointVelocities, JointPositions, JointValue 9 | from sensor_msgs.msg import JointState 10 | from geometry_msgs.msg import Point, Vector3, Quaternion 11 | 12 | # Service imports 13 | from std_srvs.srv import Empty, EmptyResponse 14 | 15 | 16 | # Python imports 17 | import numpy as np 18 | from numpy.linalg import * 19 | from math import * 20 | from PyKDL import * 21 | 22 | 23 | 24 | class poseArmController: 25 | def __init__(self, name): 26 | """ Control Framework for Youbot Arm """ 27 | self.name = name 28 | 29 | #Minimum Joint Values 30 | self.qmin = np.array([-2.9496,-1.1344,-2.6354,-1.7889,-2.9234]) 31 | #Maximum Joint Values 32 | self.qmax = np.array([2.9496,1.5707,2.5481,1.7889,2.9234]) 33 | #Offset Values -> [j0, j1, j2, j3, j4] = zeros(5) ---> Arm Stands up 34 | self.q_offset = np.array([2.9496, 1.1344, -2.5481, 1.7889, 2.9234]) 35 | #self.q_offset = np.array([2.9496, 1.1344, -2.5481, 1.7889, 0.0]) 36 | 37 | # Load parameters 38 | self.enable = True #Service Flag for enable/disable the framework 39 | 40 | #Initilize Time 41 | self.t0 = rospy.Time.now().to_sec() 42 | 43 | #Initialiaze Joint States 44 | self.q_pos = JntArray(5) #Joint Positions 45 | self.q_vel = JntArray(5) #Joint Velocities 46 | self.q_torques = JntArray(5) #Joint Torques 47 | 48 | for i in range (0,5): 49 | self.q_pos[i] = 0.0 50 | self.q_vel[i] = 0.0 51 | self.q_torques[i] = 0.0 52 | 53 | # Create publishers 54 | #Joint Position Control 55 | self.pub_qpos = rospy.Publisher("PY/arm_1/arm_controller/position_command", JointPositions) 56 | #Joint Velocity Control 57 | self.pub_qvel = rospy.Publisher("PY/arm_1/arm_controller/velocity_command", JointVelocities) 58 | 59 | # Create Subscriber 60 | #Get Joint States (position, velocity, torque) 61 | rospy.Subscriber("PY/joint_states", JointState, self.updateArmStates) 62 | 63 | 64 | #Create services 65 | self.enable_srv = rospy.Service('csc_youbot_arm/enable', Empty, self.enableSrv) 66 | self.disable_srv = rospy.Service('csc_youbot_arm/disable', Empty, self.disableSrv) 67 | 68 | 69 | def enableSrv(self, req): 70 | self.enable = True 71 | rospy.loginfo('%s Enabled', self.name) 72 | return EmptyResponse() 73 | 74 | 75 | def disableSrv(self, req): 76 | self.enable = False 77 | rospy.loginfo('%s Disabled', self.name) 78 | return EmptyResponse() 79 | 80 | def updateArmStates(self, data): 81 | if data.name[0] == 'arm_joint_1': 82 | for i in range (0, 5): 83 | self.q_pos[i] = data.position[i] - self.q_offset[i] 84 | for i in range (0, 5): 85 | self.q_vel[i] = data.velocity[i] 86 | for i in range (0, 5): 87 | self.q_torques[i] = data.effort[i] 88 | 89 | def jsPoscontrol(self,qd): 90 | 91 | #Initialize Position Command Msgs 92 | qposMsg = JointPositions() 93 | qposMsg.positions = [JointValue(), JointValue(), JointValue(), JointValue(), JointValue()] 94 | 95 | 96 | #Fill Position Msg 97 | qposMsg.positions[0].joint_uri = 'arm_joint_1' 98 | qposMsg.positions[0].unit = 'rad' 99 | qposMsg.positions[0].value = qd[0] + self.q_offset[0] 100 | 101 | qposMsg.positions[1].joint_uri = 'arm_joint_2' 102 | qposMsg.positions[1].unit = 'rad' 103 | qposMsg.positions[1].value = qd[1] + self.q_offset[1] 104 | qposMsg.positions[2].joint_uri = 'arm_joint_3' 105 | qposMsg.positions[2].unit = 'rad' 106 | qposMsg.positions[2].value = qd[2] + self.q_offset[2] 107 | 108 | qposMsg.positions[3].joint_uri = 'arm_joint_4' 109 | qposMsg.positions[3].unit = 'rad' 110 | qposMsg.positions[3].value = qd[3] + self.q_offset[3] 111 | 112 | qposMsg.positions[4].joint_uri = 'arm_joint_5' 113 | qposMsg.positions[4].unit = 'rad' 114 | qposMsg.positions[4].value = qd[4] + self.q_offset[4] 115 | 116 | #Publish Position Msg 117 | self.pub_qpos.publish(qposMsg) 118 | 119 | 120 | 121 | 122 | if __name__ == '__main__': 123 | try: 124 | rospy.init_node('poseArmController') 125 | armCTRL = poseArmController(rospy.get_name()) 126 | 127 | qd = JntArray(5) 128 | qd[0] = 0.0 129 | qd[1] = 0.0 130 | qd[2] = 0.0 131 | qd[3] = 0.0 132 | qd[4] = 0.0 133 | 134 | while not rospy.is_shutdown(): 135 | t = rospy.Time.now().to_sec() - armCTRL.t0 136 | 137 | armCTRL.jsPoscontrol(qd) 138 | rospy.sleep(1.0/300.0) 139 | rospy.spin() 140 | 141 | except rospy.ROSInterruptException: pass 142 | -------------------------------------------------------------------------------- /scripts/reconfig/poseBaseVSControllerKTH_OY.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib 5 | 6 | import rospy 7 | 8 | # Msgs imports 9 | from geometry_msgs.msg import Twist, Quaternion, Vector3 10 | 11 | # Service imports 12 | from std_srvs.srv import Empty, EmptyResponse 13 | from ar_pose.msg import * 14 | from ltl3_NTUA.msg import * 15 | # Python imports 16 | import numpy as np 17 | from numpy.linalg import * 18 | import math as math1 19 | 20 | #Custom imports 21 | import sys 22 | from math import floor, exp, sqrt, atan2, sin, pi, asin, cos 23 | from geometry_msgs.msg import Point, Vector3, Quaternion 24 | 25 | header = -1 26 | 27 | class poseBaseVSController: 28 | def __init__(self, name): 29 | """ Position Based Visual Servo Control for the Youbot Base """ 30 | self.name = name 31 | 32 | # Load parameters 33 | self.enable = True #Service Flag for enable/disable the controller 34 | self.counter = 0 35 | 36 | #Initilize Time 37 | self.t0 = rospy.Time.now().to_sec() 38 | 39 | #Initialiaze platform States 40 | self.pose1 = np.zeros(3,dtype=float) #Youbot Base 1 41 | self.pose2 = np.zeros(3,dtype=float) #Youbot Base 2 42 | 43 | #Initialize desired pose states 44 | self.posed = np.zeros(3,dtype=float) #For the time Youbot 1 (IP:147.102.51.243) 45 | 46 | #Desired pose given in terminal 47 | self.posed[0] = 0.0 #(float (sys.argv[1])) 48 | self.posed[1] = 0.0 #(float (sys.argv[2])) 49 | self.posed[2] = 0.0 #(float (sys.argv[3])) 50 | 51 | # Create publishers 52 | self.pub_vel = rospy.Publisher('/oy/cmd_vel', Twist) 53 | self.pub_confirm = rospy.Publisher('activity_done_OY', confirmation) 54 | 55 | # Create Subscriber 56 | #rosOY.Subscriber("/cur_pose_C", pose, self.updatePose) 57 | rospy.Subscriber("/ceiling/pose", ARMarkersExtended, self.updateRobotsPose) 58 | rospy.Subscriber("/next_move_OY", activity, self.updateNextMove) 59 | 60 | #Create services 61 | self.enable_srv = rospy.Service('/youbot_base_control/enable', Empty, self.enableSrv) 62 | self.disable_srv = rospy.Service('/youbot_base_control/disable', Empty, self.disableSrv) 63 | 64 | 65 | self.xd_old = 0.0 66 | self.yd_old = 0.0 67 | 68 | self.Action = '' 69 | self.flag_conf = False 70 | 71 | def updateNextMove(self, data): 72 | self.Action = data.type 73 | global header 74 | 75 | if data.header != header: 76 | self.flag_conf = True 77 | header = data.header 78 | 79 | if data.type == 'goto' and self.flag_conf == True: 80 | self.posed[0] = data.x 81 | self.posed[1] = -data.y 82 | self.posed[2] = data.psi 83 | 84 | 85 | def updateRobotsPose(self, m): 86 | 87 | for poses in m.markers: 88 | 89 | if poses.id == 0: 90 | #print "updating pose 1" 91 | self.pose1[0] = poses.pose.pose.position.x 92 | self.pose1[1] = -poses.pose.pose.position.y 93 | 94 | quat1 = Quaternion(0,0,poses.pose.pose.orientation.y,poses.pose.pose.orientation.x) 95 | euler1 = self.quat2eulerZYX (quat1) 96 | self.pose1[2] = -euler1.z 97 | 98 | 99 | if poses.id == 1: 100 | #print "updating pose 0" 101 | self.pose2[0]= poses.pose.pose.position.x 102 | self.pose2[1] = -poses.pose.pose.position.y 103 | 104 | quat2 = Quaternion(0,0,poses.pose.pose.orientation.y,poses.pose.pose.orientation.x) 105 | euler2 = self.quat2eulerZYX (quat2) 106 | self.pose2[2] = -euler2.z 107 | 108 | 109 | #Quaternion to Euler-ZYX 110 | def quat2eulerZYX (self,q): 111 | euler = Vector3() 112 | tol = self.quat2eulerZYX.tolerance 113 | 114 | qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z 115 | qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z 116 | qwy, qwz = q.w*q.y, q.w*q.z 117 | 118 | test = -2.0 * (qxz - qwy) 119 | if test > +tol: 120 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 121 | euler.y = +0.5 * pi 122 | euler.z = 0.0 123 | 124 | return euler 125 | 126 | elif test < -tol: 127 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 128 | euler.y = -0.5 * pi 129 | euler.z = tol 130 | 131 | return euler 132 | 133 | else: 134 | euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz) 135 | euler.y = asin (test) 136 | euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz) 137 | 138 | return euler 139 | quat2eulerZYX.tolerance=0.99999 140 | 141 | 142 | def enableSrv(self, req): 143 | self.enable = True 144 | rospy.loginfo('%s Enabled', self.name) 145 | return EmptyResponse() 146 | 147 | 148 | def disableSrv(self, req): 149 | self.enable = False 150 | rospy.loginfo('%s Disabled', self.name) 151 | return EmptyResponse() 152 | 153 | 154 | def motionControl(self): 155 | 156 | x = self.pose1[0] 157 | y = self.pose1[1] 158 | psi = self.pose1[2] 159 | 160 | x_d = self.posed[0] 161 | y_d = self.posed[1] 162 | psi_d = self.posed[2] 163 | 164 | #Pose Errors 165 | e_x = x - x_d 166 | e_y = y - y_d 167 | e_psi = psi - psi_d 168 | 169 | #rospy.loginfo("ex: %s ey: %s epsi: %s",e_x, e_y, e_psi) 170 | 171 | #Controller Gains 172 | k_x = 0.15 173 | k_y = 0.15 174 | k_psi = 0.15 175 | 176 | #Motion Control Scheme 177 | u = -k_x*e_x*cos(psi) - k_y*e_y*sin(psi) 178 | v = k_x*e_x*sin(psi) - k_y*e_y*cos(psi) 179 | r = -k_psi*e_psi 180 | 181 | cmdMsg = Twist() 182 | 183 | #u = 0.0 184 | #v = 0.0 185 | #r = 0.0 186 | 187 | #print "x:", x, "y:", y, "psi:", psi 188 | 189 | criteria1 = 0.1 190 | criteria2 = 0.03 191 | 192 | norm1 = sqrt(e_x**2+e_y**2) 193 | norm2 = abs(e_psi) 194 | 195 | if norm1 < criteria1 and norm2 < criteria2 and self.flag_conf == True: 196 | self.flag_conf = False 197 | confMsg = confirmation() 198 | confMsg.name = 'goto' 199 | confMsg.done = 1 200 | print '==============================================' 201 | confMsg.confheader = header 202 | print header 203 | print confMsg.done 204 | self.pub_confirm.publish(confMsg) 205 | 206 | #Publish Position Msg 207 | if self.Action == 'goto': 208 | cmdMsg.linear.x = u 209 | cmdMsg.linear.y = v 210 | cmdMsg.angular.z = r 211 | else: 212 | cmdMsg.linear.x = 0.0 213 | cmdMsg.linear.y = 0.0 214 | cmdMsg.angular.z = 0.0 215 | #print "I am in publishing", u, v, r 216 | 217 | self.pub_vel.publish(cmdMsg) 218 | 219 | if __name__ == '__main__': 220 | try: 221 | rospy.init_node('poseBaseVSController_OY') 222 | 223 | poseBaseObj = poseBaseVSController(rospy.get_name()) 224 | while not rospy.is_shutdown(): 225 | t = rospy.Time.now().to_sec() - poseBaseObj.t0 226 | poseBaseObj.motionControl() 227 | rospy.sleep(1.0/10.0) 228 | 229 | except rospy.ROSInterruptException: pass 230 | pose 231 | -------------------------------------------------------------------------------- /scripts/reconfig/poseBaseVSControllerKTH_PY.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | 3 | # ROS imports 4 | import roslib 5 | 6 | import rospy 7 | 8 | # Msgs imports 9 | from geometry_msgs.msg import Twist, Quaternion, Vector3 10 | 11 | # Service imports 12 | from std_srvs.srv import Empty, EmptyResponse 13 | from ar_pose.msg import * 14 | from ltl3_NTUA.msg import * 15 | # Python imports 16 | import numpy as np 17 | from numpy.linalg import * 18 | import math as math1 19 | 20 | #Custom imports 21 | import sys 22 | from math import floor, exp, sqrt, atan2, sin, pi, asin, cos 23 | from geometry_msgs.msg import Point, Vector3, Quaternion 24 | 25 | header = -1 26 | 27 | class poseBaseVSController: 28 | def __init__(self, name): 29 | """ Position Based Visual Servo Control for the Youbot Base """ 30 | self.name = name 31 | 32 | # Load parameters 33 | self.enable = True #Service Flag for enable/disable the controller 34 | self.counter = 0 35 | 36 | #Initilize Time 37 | self.t0 = rospy.Time.now().to_sec() 38 | 39 | #Initialiaze platform States 40 | self.pose1 = np.zeros(3,dtype=float) #Youbot Base 1 41 | self.pose2 = np.zeros(3,dtype=float) #Youbot Base 2 42 | 43 | #Initialize desired pose states 44 | self.posed = np.zeros(3,dtype=float) #For the time Youbot 1 (IP:147.102.51.243) 45 | 46 | #Desired pose given in terminal 47 | self.posed[0] = 0.0 #(float (sys.argv[1])) 48 | self.posed[1] = 0.0 #(float (sys.argv[2])) 49 | self.posed[2] = 0.0 #(float (sys.argv[3])) 50 | 51 | # Create publishers 52 | self.pub_vel = rospy.Publisher('/py/cmd_vel', Twist) 53 | self.pub_confirm = rospy.Publisher('activity_done_PY', confirmation) 54 | 55 | # Create Subscriber 56 | #rosOY.Subscriber("/cur_pose_C", pose, self.updatePose) 57 | rospy.Subscriber("/ceiling/pose", ARMarkersExtended, self.updateRobotsPose) 58 | rospy.Subscriber("/next_move_PY", activity, self.updateNextMove) 59 | 60 | #Create services 61 | self.enable_srv = rospy.Service('/youbot_base_control/enable', Empty, self.enableSrv) 62 | self.disable_srv = rospy.Service('/youbot_base_control/disable', Empty, self.disableSrv) 63 | 64 | 65 | self.xd_old = 0.0 66 | self.yd_old = 0.0 67 | 68 | self.Action = '' 69 | self.flag_conf = False 70 | 71 | def updateNextMove(self, data): 72 | self.Action = data.type 73 | global header 74 | 75 | if data.header != header: 76 | self.flag_conf = True 77 | header = data.header 78 | 79 | if data.type == 'goto' and self.flag_conf == True: 80 | self.posed[0] = data.x 81 | self.posed[1] = -data.y 82 | self.posed[2] = data.psi 83 | 84 | 85 | def updateRobotsPose(self, m): 86 | 87 | for poses in m.markers: 88 | 89 | if poses.id == 0: 90 | #print "updating pose 1" 91 | self.pose1[0] = poses.pose.pose.position.x 92 | self.pose1[1] = -poses.pose.pose.position.y 93 | 94 | quat1 = Quaternion(0,0,poses.pose.pose.orientation.y,poses.pose.pose.orientation.x) 95 | euler1 = self.quat2eulerZYX (quat1) 96 | self.pose1[2] = -euler1.z 97 | 98 | 99 | if poses.id == 1: 100 | #print "updating pose 0" 101 | self.pose2[0]= poses.pose.pose.position.x 102 | self.pose2[1] = -poses.pose.pose.position.y 103 | 104 | quat2 = Quaternion(0,0,poses.pose.pose.orientation.y,poses.pose.pose.orientation.x) 105 | euler2 = self.quat2eulerZYX (quat2) 106 | self.pose2[2] = -euler2.z 107 | 108 | 109 | #Quaternion to Euler-ZYX 110 | def quat2eulerZYX (self,q): 111 | euler = Vector3() 112 | tol = self.quat2eulerZYX.tolerance 113 | 114 | qww, qxx, qyy, qzz = q.w*q.w, q.x*q.x, q.y*q.y, q.z*q.z 115 | qwx, qxy, qyz, qxz= q.w*q.x, q.x*q.y, q.y*q.z, q.x*q.z 116 | qwy, qwz = q.w*q.y, q.w*q.z 117 | 118 | test = -2.0 * (qxz - qwy) 119 | if test > +tol: 120 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 121 | euler.y = +0.5 * pi 122 | euler.z = 0.0 123 | 124 | return euler 125 | 126 | elif test < -tol: 127 | euler.x = atan2 (-2.0*(qyz-qwx), qww-qxx+qyy-qzz) 128 | euler.y = -0.5 * pi 129 | euler.z = tol 130 | 131 | return euler 132 | 133 | else: 134 | euler.x = atan2 (2.0*(qyz+qwx), qww-qxx-qyy+qzz) 135 | euler.y = asin (test) 136 | euler.z = atan2 (2.0*(qxy+qwz), qww+qxx-qyy-qzz) 137 | 138 | return euler 139 | quat2eulerZYX.tolerance=0.99999 140 | 141 | 142 | def enableSrv(self, req): 143 | self.enable = Truef 144 | rospy.loginfo('%s Enabled', self.name) 145 | return EmptyResponse() 146 | 147 | 148 | def disableSrv(self, req): 149 | self.enable = False 150 | rospy.loginfo('%s Disabled', self.name) 151 | return EmptyResponse() 152 | 153 | 154 | def motionControl(self): 155 | 156 | x = self.pose2[0] 157 | y = self.pose2[1] 158 | psi = self.pose2[2] 159 | 160 | x_d = self.posed[0] 161 | y_d = self.posed[1] 162 | psi_d = self.posed[2] 163 | 164 | #Pose Errors 165 | e_x = x - x_d 166 | e_y = y - y_d 167 | e_psi = psi - psi_d 168 | 169 | #rospy.loginfo("ex: %s ey: %s epsi: %s",e_x, e_y, e_psi) 170 | 171 | #Controller Gains 172 | k_x = 0.15 173 | k_y = 0.15 174 | k_psi = 0.15 175 | 176 | #Motion Control Scheme 177 | u = -k_x*e_x*cos(psi) - k_y*e_y*sin(psi) 178 | v = k_x*e_x*sin(psi) - k_y*e_y*cos(psi) 179 | r = -k_psi*e_psi 180 | 181 | cmdMsg = Twist() 182 | 183 | #u = 0.0 184 | #v = 0.0 185 | #r = 0.0 186 | 187 | #print "x:", x, "y:", y, "psi:", psi 188 | 189 | criteria1 = 0.2 190 | criteria2 = 0.05 191 | 192 | norm1 = sqrt(e_x**2+e_y**2) 193 | norm2 = abs(e_psi) 194 | 195 | if norm1 < criteria1 and norm2 < criteria2 and self.flag_conf == True: 196 | self.flag_conf = False 197 | confMsg = confirmation() 198 | confMsg.name = 'goto' 199 | confMsg.done = 1 200 | print '==============================================' 201 | confMsg.confheader = header 202 | print header 203 | print confMsg.done 204 | self.pub_confirm.publish(confMsg) 205 | 206 | #Publish Position Msg 207 | if self.Action == 'goto': 208 | cmdMsg.linear.x = u 209 | cmdMsg.linear.y = v 210 | cmdMsg.angular.z = r 211 | else: 212 | cmdMsg.linear.x = 0.0 213 | cmdMsg.linear.y = 0.0 214 | cmdMsg.angular.z = 0.0 215 | #print "I am in publishing", u, v, r 216 | 217 | self.pub_vel.publish(cmdMsg) 218 | 219 | if __name__ == '__main__': 220 | try: 221 | rospy.init_node('poseBaseVSController_PY') 222 | 223 | poseBaseObj = poseBaseVSController(rospy.get_name()) 224 | while not rospy.is_shutdown(): 225 | t = rospy.Time.now().to_sec() - poseBaseObj.t0 226 | poseBaseObj.motionControl() 227 | rospy.sleep(1.0/10.0) 228 | 229 | except rospy.ROSInterruptException: pass 230 | pose 231 | -------------------------------------------------------------------------------- /scripts/reconfig/relative_nao.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('ltl3_NTUA') 4 | import rospy 5 | import Queue 6 | import numpy 7 | 8 | import math 9 | import tf 10 | from tf.transformations import euler_from_quaternion 11 | 12 | from ar_pose.msg import ARMarkerExtended, ARMarkersExtended 13 | from ltl3_NTUA.msg import pose, activity, confirmation, knowledge 14 | from math import sqrt, cos, sin, radians 15 | from numpy.linalg import inv 16 | 17 | def pose_callback(data): 18 | global ac_POSE 19 | global POSE 20 | 21 | pose = [0, 0, 0] # x, y, theta 22 | for marker in data.markers: 23 | if marker.id == 2: # ID for NAO marker 24 | pose[0] = marker.pose.pose.position.x 25 | pose[1] = marker.pose.pose.position.y 26 | angle = euler_from_quaternion([marker.pose.pose.orientation.x, 27 | marker.pose.pose.orientation.y, marker.pose.pose.orientation.z, 28 | marker.pose.pose.orientation.w]) 29 | pose[2] = angle[2] 30 | 31 | while ac_POSE.qsize() < 5: 32 | # print "while loop" 33 | ac_POSE.put(pose) 34 | # print "inserting to FIFO queue", [data.x, data.y, data.theta] 35 | else: 36 | #print "else" 37 | ac_POSE.get() 38 | ac_POSE.put(pose) 39 | # print "inserting to FIFO queue", [data.x, data.y, data.theta] 40 | # print "qsize", ac_POSE.qsize() 41 | POSE = median_filter(ac_POSE) 42 | 43 | 44 | def median_filter(raw_pose): 45 | list_pose = list(raw_pose.queue) 46 | x_list = [pose[0] for pose in list_pose] 47 | median_x = numpy.median(x_list) 48 | y_list = [pose[1] for pose in list_pose] 49 | median_y = numpy.median(y_list) 50 | theta_list = [pose[2] for pose in list_pose] 51 | median_theta = numpy.median(theta_list) 52 | return [median_x, median_y, median_theta] 53 | 54 | def activity_callback(data): 55 | global goal_loc 56 | goal_loc[0] = data.x 57 | goal_loc[1] = data.y 58 | goal_loc[2] = data.psi 59 | goal_loc[3] = data.header 60 | return goal_loc 61 | 62 | 63 | def rotate_3d_vector(theta, tr_x, tr_y, x_goal, y_goal): 64 | new_v = [0.0, 0.0, 0.0]; 65 | T_G_new_N = numpy.array([[cos(theta), sin(theta), tr_x], [-sin(theta), cos(theta), tr_y], [0.0, 0.0, 1] ]); 66 | T_G_new_N_inv = inv(T_G_new_N); 67 | goal_pos_tmp = numpy.array([x_goal, y_goal, 1]); 68 | new_v = T_G_new_N_inv.dot(goal_pos_tmp); 69 | #new_v[0] = cos(theta)*v[0] + sin(theta)*v[1] + tr_x*v[2] 70 | #new_v[1] = -sin(theta)*v[0] + cos(theta)*v[1] + tr_y*v[2] 71 | #new_v[2] = 0.0*v[0] + 0.0*v[1] + 1.0*v[2] 72 | return new_v 73 | 74 | if __name__ == '__main__': 75 | global POSE 76 | global goal_loc 77 | global ac_POSE 78 | 79 | ac_POSE = Queue.Queue(5) 80 | POSE = [0, 0, 0] 81 | goal_loc = [0, 0, 0, 0] 82 | relative_goal = [0, 0, 0] 83 | 84 | rospy.init_node('relative_nao') 85 | 86 | relative_nao_pub = rospy.Publisher('next_move_PY_relative',activity) 87 | nao_confirm_pub = rospy.Publisher('activity_done_PY',confirmation) 88 | 89 | rospy.Subscriber('ceiling/pose', ARMarkersExtended, pose_callback) 90 | rospy.Subscriber('next_move_PY', activity, activity_callback) 91 | 92 | rate = rospy.Rate(10.0) 93 | while not rospy.is_shutdown(): 94 | #while (goal_loc[0] != 0) or (goal_loc[1] != 0): 95 | x_nao_G = POSE[0]; 96 | y_nao_G = POSE[1]; 97 | x_nao_G_new = x_nao_G; 98 | y_nao_G_new = -y_nao_G; 99 | x_goal_G = goal_loc[0]; 100 | y_goal_G = goal_loc[1]; 101 | x_goal_G_new = x_goal_G; 102 | y_goal_G_new = -y_goal_G; 103 | 104 | print "xy_nao_G", x_nao_G, y_nao_G; 105 | print "xy_nao_G_new", x_nao_G_new, y_nao_G_new; 106 | error_vector = rotate_3d_vector(POSE[2], x_nao_G_new, y_nao_G_new, x_goal_G_new, y_goal_G_new) 107 | #print error_vector 108 | print "xy_goal_N", error_vector; 109 | #relative_goal[0] = goal_loc[0] - POSE[0] 110 | #relative_goal[1] = goal_loc[1] - POSE[1] 111 | #relative_goal[2] = goal_loc[2] - POSE[2] 112 | #oriented_relative_pose = rotate_2d_vector(relative_goal, -POSE[2]) 113 | relative_nao_msg = activity() 114 | relative_nao_msg.type = '' 115 | #relative_nao_msg.header = goal_loc[3] 116 | relative_nao_msg.x = 100*error_vector[0];#oriented_relative_pose[0]*100 117 | relative_nao_msg.y = 100*error_vector[1];#oriented_relative_pose[1]*100 118 | relative_psi = goal_loc[2] - POSE[2] 119 | relative_psi_to_pi = ( relative_psi + numpy.pi) % (2 * numpy.pi ) - numpy.pi 120 | relative_nao_msg.psi = relative_psi_to_pi 121 | relative_nao_pub.publish(relative_nao_msg) 122 | print "errors (x,y,theta)", numpy.absolute(relative_nao_msg.x), numpy.absolute(relative_nao_msg.y), numpy.absolute(relative_nao_msg.psi); 123 | if (numpy.absolute(relative_nao_msg.x) <= 20.0 and numpy.absolute(relative_nao_msg.y) <= 20.0 and numpy.absolute(relative_nao_msg.psi) <= 0.4): 124 | print "sending confirmation now" 125 | #print error_vector 126 | nao_confirm_msg = confirmation() 127 | nao_confirm_msg.confheader = goal_loc[3] 128 | nao_confirm_msg.name = 'goto' 129 | nao_confirm_msg.done = 1 130 | nao_confirm_pub.publish(nao_confirm_msg) 131 | #print 'realtive_nao, (x,y)', (relative_nao_msg.x,relative_nao_msg.y) 132 | #rospy.sleep(0.5) 133 | rate.sleep() 134 | -------------------------------------------------------------------------------- /scripts/reconfig/tf2pose.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('ltl3') 4 | import rospy 5 | from ltl3.msg import pose 6 | import math 7 | import tf 8 | from tf.transformations import euler_from_quaternion 9 | 10 | ROS_TO_MAZE = 166 11 | RAD_TO_DEG = 57.3 12 | 13 | if __name__ == '__main__': 14 | rospy.init_node('tf2pose') 15 | listener = tf.TransformListener() 16 | pose_pub_B = rospy.Publisher('cur_pose_B', pose) 17 | pose_pub_C = rospy.Publisher('cur_pose_C', pose) 18 | rate = rospy.Rate(10.0) 19 | while not rospy.is_shutdown(): 20 | try: 21 | (trans_B,rot_B) = listener.lookupTransform('data/multi/patt.letter_a', 'data/multi/patt.letter_b', rospy.Time(0)) 22 | (trans_C,rot_C) = listener.lookupTransform('data/multi/patt.letter_a', 'data/multi/patt.letter_c', rospy.Time(0)) 23 | # trans, (dx, dy, dz) 24 | # rot, (x,y,z,w) should be [theta,0,0,1] 25 | except (tf.LookupException, tf.ConnectivityException, tf.ExtrapolationException): 26 | continue 27 | # roatation round x, y, z axies, for letter "B" 28 | angles_B= euler_from_quaternion(rot_B) 29 | cur_pose_B = pose() 30 | cur_pose_B.x = trans_B[0]*ROS_TO_MAZE # in cm 31 | cur_pose_B.y = trans_B[1]*ROS_TO_MAZE # in cm 32 | cur_pose_B.theta = angles_B[2] # in radian 33 | info = 'letter B position: X:%s, Y:%s, Theta:%s' %(cur_pose_B.x, cur_pose_B.y, cur_pose_B.theta*RAD_TO_DEG) 34 | rospy.loginfo(info) 35 | pose_pub_B.publish(cur_pose_B) 36 | # roatation round x, y, z axies, for letter "C" 37 | angles_C= euler_from_quaternion(rot_C) 38 | cur_pose_C = pose() 39 | cur_pose_C.x = trans_C[0]*ROS_TO_MAZE # in cm 40 | cur_pose_C.y = trans_C[1]*ROS_TO_MAZE # in cm 41 | cur_pose_C.theta = angles_C[2] # in radian 42 | info = 'letter C position: X:%s, Y:%s, Theta:%s' %(cur_pose_C.x, cur_pose_C.y, cur_pose_C.theta*RAD_TO_DEG) 43 | rospy.loginfo(info) 44 | pose_pub_C.publish(cur_pose_C) 45 | ######## 46 | rate.sleep() 47 | -------------------------------------------------------------------------------- /scripts/reconfig/tf2pose_for_two.py: -------------------------------------------------------------------------------- 1 | #!/usr/bin/env python 2 | import roslib 3 | roslib.load_manifest('ltl3') 4 | import rospy 5 | from ltl3.msg import pose 6 | import math 7 | import tf 8 | from tf.transformations import euler_from_quaternion 9 | 10 | ROS_TO_MAZE = 166 11 | RAD_TO_DEG = 57.3 12 | 13 | if __name__ == '__main__': 14 | rospy.init_node('tf2pose') 15 | listener = tf.TransformListener() 16 | pose_pub_B = rospy.Publisher('cur_pose_B', pose) 17 | pose_pub_C = rospy.Publisher('cur_pose_C', pose) 18 | rate = rospy.Rate(10.0) 19 | while not rospy.is_shutdown(): 20 | try: 21 | listener.waitForTransform('data/multi/patt.letter_a1', 'data/multi/patt.letter_b1', rospy.Time(0),rospy.Duration(0.2)) 22 | (trans_B,rot_B) = listener.lookupTransform('data/multi/patt.letter_a1', 'data/multi/patt.letter_b1', rospy.Time(0)) 23 | #(trans_C,rot_C) = listener.lookupTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_c2', rospy.Time(0)) 24 | # trans, (dx, dy, dz) 25 | # rot, (x,y,z,w) should be [theta,0,0,1] 26 | except tf.Exception: 27 | try: 28 | listener.waitForTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_b2', rospy.Time(0),rospy.Duration(0.2)) 29 | (trans_B,rot_B) = listener.lookupTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_b2', rospy.Time(0)) 30 | except tf.Exception: 31 | info = 'No such letter found!' 32 | rospy.loginfo(info) 33 | try: 34 | listener.waitForTransform('data/multi/patt.letter_a1', 'data/multi/patt.letter_c1', rospy.Time(0),rospy.Duration(0.2)) 35 | (trans_C,rot_C) = listener.lookupTransform('data/multi/patt.letter_a1', 'data/multi/patt.letter_c1', rospy.Time(0)) 36 | #(trans_C,rot_C) = listener.lookupTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_c2', rospy.Time(0)) 37 | # trans, (dx, dy, dz) 38 | # rot, (x,y,z,w) should be [theta,0,0,1] 39 | except tf.Exception: 40 | try: 41 | listener.waitForTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_c2', rospy.Time(0),rospy.Duration(0.2)) 42 | (trans_C,rot_C) = listener.lookupTransform('data/multi/patt.letter_a2', 'data/multi/patt.letter_c2', rospy.Time(0)) 43 | except tf.Exception: 44 | info = 'No such letter found!' 45 | rospy.loginfo(info) 46 | ############################################# 47 | # roatation round x, y, z axies, for letter "B" 48 | angles_B= euler_from_quaternion(rot_B) 49 | cur_pose_B = pose() 50 | cur_pose_B.x = trans_B[0]*ROS_TO_MAZE # in cm 51 | cur_pose_B.y = trans_B[1]*ROS_TO_MAZE # in cm 52 | cur_pose_B.theta = angles_B[2] # in radian 53 | info = 'letter B position: X:%s, Y:%s, Theta:%s' %(cur_pose_B.x, cur_pose_B.y, cur_pose_B.theta*RAD_TO_DEG) 54 | rospy.loginfo(info) 55 | pose_pub_B.publish(cur_pose_B) 56 | ############################################# 57 | # roatation round x, y, z axies, for letter "C" 58 | angles_C= euler_from_quaternion(rot_C) 59 | cur_pose_C = pose() 60 | cur_pose_C.x = trans_C[0]*ROS_TO_MAZE # in cm 61 | cur_pose_C.y = trans_C[1]*ROS_TO_MAZE # in cm 62 | cur_pose_C.theta = angles_C[2] # in radian 63 | info = 'letter C position: X:%s, Y:%s, Theta:%s' %(cur_pose_C.x, cur_pose_C.y, cur_pose_C.theta*RAD_TO_DEG) 64 | rospy.loginfo(info) 65 | pose_pub_C.publish(cur_pose_C) 66 | ######## 67 | rate.sleep() 68 | -------------------------------------------------------------------------------- /src/client/display.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/display.h" 2 | 3 | static GLuint font_base; 4 | extern NodeRoot root; 5 | bool* keyStates = new bool[256]; 6 | bool* keySpecialStates = new bool[246]; 7 | 8 | // functions to manage keystrokes (pressed down) 9 | void keyPressed (unsigned char key, int x, int y) 10 | { 11 | std::cout << "Key Value: " << key << std::endl; 12 | keyStates[key] = true; 13 | } 14 | 15 | // functions to manage keystrokes (released ) 16 | void keyUp (unsigned char key, int x, int y) 17 | { 18 | keyStates[key] = false; 19 | } 20 | 21 | // functions to manage special keystrokes (pressed down) 22 | void keySpecial (int key, int x, int y) 23 | { 24 | std::cout << "Special Key Value: " << key << std::endl; 25 | keySpecialStates[key] = true; 26 | } 27 | 28 | // functions to manage special keystrokes (released) 29 | void keySpecialUp (int key, int x, int y) 30 | { 31 | keySpecialStates[key] = false; 32 | } 33 | 34 | // draw the bt node's current status using a frame (color coded) 35 | void draw_status(GLfloat x, GLfloat y, STATE node_status) 36 | { 37 | switch (node_status) 38 | { 39 | case SUCCESS: glColor3f(0.0, 1.0, 0.0); break; 40 | case FAILURE: glColor3f(1.0, 0.0, 0.0); break; 41 | case RUNNING: glColor3f(1.0, 1.0, 0.0); break; 42 | case NODE_ERROR: glColor3f(1.0, 1.0, 1.0); break; 43 | default: std::cout << "Error Node Status Color Selection" << std::cout; 44 | } 45 | glLineWidth(3.0); 46 | glBegin(GL_LINE_LOOP); 47 | glVertex3f((GLfloat) (x + STATUS_WIDTH), (GLfloat) (y - STATUS_HEIGHT), (GLfloat) 0.0); 48 | glVertex3f((GLfloat) (x + STATUS_WIDTH), (GLfloat) (y + STATUS_HEIGHT), (GLfloat) 0.0); 49 | glVertex3f((GLfloat) (x - STATUS_WIDTH), (GLfloat) (y + STATUS_HEIGHT), (GLfloat) 0.0); 50 | glVertex3f((GLfloat) (x - STATUS_WIDTH), (GLfloat) (y - STATUS_HEIGHT), (GLfloat) 0.0); 51 | glEnd(); 52 | glLineWidth(1.0); 53 | } 54 | 55 | // draw the cursor where the user is standing on the bt using a big frame (white) 56 | void draw_cursor(GLfloat x, GLfloat y) 57 | { 58 | glLineWidth(3.0); 59 | glBegin(GL_LINE_LOOP); 60 | glColor3f(1.0, 1.0, 0.0); 61 | glVertex3f((GLfloat) (x + CURSOR_WIDTH), (GLfloat) (y - CURSOR_HEIGHT), (GLfloat) 0.0); 62 | glVertex3f((GLfloat) (x + CURSOR_WIDTH), (GLfloat) (y + CURSOR_HEIGHT), (GLfloat) 0.0); 63 | glVertex3f((GLfloat) (x - CURSOR_WIDTH), (GLfloat) (y + CURSOR_HEIGHT), (GLfloat) 0.0); 64 | glVertex3f((GLfloat) (x - CURSOR_WIDTH), (GLfloat) (y - CURSOR_HEIGHT), (GLfloat) 0.0); 65 | glEnd(); 66 | glLineWidth(1.0); 67 | } 68 | 69 | // draw the node itself using a solid square (color coded) 70 | void draw_node(GLfloat x, GLfloat y, NODE_TYPE node_type) 71 | { 72 | switch (node_type) 73 | { 74 | case SELECTOR: glColor3f(1.0, 0.0, 0.0); break; 75 | case SELECTOR_STAR: glColor3f(0.6, 0.0, 0.0); break; 76 | case SEQUENCE: glColor3f(0.0, 0.0, 1.0); break; 77 | case SEQUENCE_STAR: glColor3f(0.0, 0.0, 0.6); break; 78 | case PARALLEL: glColor3f(0.0, 1.0, 1.0); break; 79 | case DECORATOR: glColor3f(1.0, 0.0, 1.0); break; 80 | case ACTION: glColor3f(0.0, 1.0, 0.0); break; 81 | case CONDITION: glColor3f(1.0, 1.0, 0.0); break; 82 | case ROOT: glColor3f(1.0, 1.0, 1.0); break; 83 | default: std::cout << "Invalid Node Selected (color)" << std::endl; 84 | } 85 | glBegin(GL_QUADS); 86 | glVertex3f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT), (GLfloat) 0.0); 87 | glVertex3f((GLfloat) (x + NODE_WIDTH), (GLfloat) (y + NODE_HEIGHT), (GLfloat) 0.0); 88 | glVertex3f((GLfloat) (x - NODE_WIDTH), (GLfloat) (y + NODE_HEIGHT), (GLfloat) 0.0); 89 | glVertex3f((GLfloat) (x - NODE_WIDTH), (GLfloat) (y - NODE_HEIGHT), (GLfloat) 0.0); 90 | glEnd(); 91 | } 92 | 93 | // draw the edge connecting one node to the other 94 | void draw_connector(GLfloat parent_x, GLfloat parent_y, GLfloat x, GLfloat y) 95 | { 96 | glBegin(GL_LINES); 97 | glColor3f(1.0, 1.0, 1.0); 98 | glVertex2f(parent_x, parent_y); 99 | glVertex2f(x, y); 100 | glEnd(); 101 | } 102 | 103 | // draws the whole tree, cursor, and statuses, calls swap buffers 104 | void draw_all() 105 | { 106 | std::cout << "Drawing Everything" << std::endl; 107 | glClear(GL_COLOR_BUFFER_BIT); 108 | root.draw_subtree((GLfloat) (CENTER_ROW), (GLfloat) (FIRST_LINE), 1, 109 | (GLfloat) (SCREEN_WIDTH)); 110 | glutSwapBuffers(); 111 | } 112 | 113 | // initialization of the Glut parameters, projection type, screen size 114 | void initialize() 115 | { 116 | glClearColor(0.0, 0.0, 0.0, 0.0); 117 | glMatrixMode(GL_PROJECTION); 118 | glLoadIdentity(); 119 | glOrtho(0, SCREEN_WIDTH, 0, SCREEN_HEIGHT, -1.0, 1.0); 120 | } 121 | 122 | // more general initialization which includes the keyboard listener 123 | void glut_setup(int iArgc, char** cppArgv) 124 | { 125 | std::string font_name_str = "fixed"; 126 | char* font_name = (char*)font_name_str.c_str(); 127 | 128 | glutInit(&iArgc, cppArgv); 129 | glutInitDisplayMode(GLUT_DOUBLE | GLUT_RGB); 130 | glutInitWindowSize((int) (SCREEN_WIDTH), (int) (SCREEN_HEIGHT)); 131 | glutInitWindowPosition(SCREEN_POSITION_X, SCREEN_POSITION_Y); 132 | glutCreateWindow(WINDOWS_NAME); 133 | 134 | initialize(); 135 | my_init(font_name); 136 | 137 | if (FULLSCREEN) 138 | glutFullScreen(); 139 | glutDisplayFunc(draw_all); // function called to display 140 | // glutReshapeFunc(my_reshape); 141 | 142 | glutKeyboardFunc(keyPressed); 143 | glutKeyboardUpFunc(keyUp); 144 | glutSpecialFunc(keySpecial); 145 | glutSpecialUpFunc(keySpecialUp); 146 | } 147 | 148 | // process called to process events 149 | void glut_process() 150 | { 151 | glutMainLoopEvent(); 152 | } 153 | 154 | // load fonts to display the node names 155 | void init_font(GLuint base, const char* f) 156 | { 157 | Display* display; 158 | XFontStruct* font_info; 159 | int first; 160 | int last; 161 | 162 | display = XOpenDisplay(0); 163 | if (display == 0) 164 | { 165 | fprintf(stderr, "XOpenDisplay() failed. Exiting.\n"); 166 | exit(-1); 167 | } 168 | else 169 | { 170 | font_info = XLoadQueryFont(display, f); 171 | if (!font_info) 172 | { 173 | fprintf(stderr, "XLoadQueryFont() failed - Exiting.\n"); 174 | exit(-1); 175 | } 176 | else 177 | { 178 | first = font_info->min_char_or_byte2; 179 | last = font_info->max_char_or_byte2; 180 | glXUseXFont(font_info->fid, first, last-first+1, base+first); 181 | } 182 | XCloseDisplay(display); 183 | display = 0; 184 | } 185 | } 186 | 187 | // print a string in OpenGL 188 | void print_string(GLuint base, const char* s) 189 | { 190 | if (!glIsList(font_base)) 191 | { 192 | fprintf(stderr, "print_string(): Bad display list. - Exiting.\n"); 193 | exit (-1); 194 | } 195 | else if (s && strlen(s)) 196 | { 197 | glPushAttrib(GL_LIST_BIT); 198 | glListBase(base); 199 | glCallLists(strlen(s), GL_UNSIGNED_BYTE, (GLubyte *)s); 200 | glPopAttrib(); 201 | } 202 | } 203 | 204 | // load font to be used with OpenGL 205 | void my_init(char* f) 206 | { 207 | font_base = glGenLists(256); 208 | if (!glIsList(font_base)) 209 | { 210 | fprintf(stderr, "my_init(): Out of display lists. - Exiting.\n"); 211 | exit (-1); 212 | } 213 | else 214 | { 215 | init_font(font_base, f); 216 | } 217 | } 218 | 219 | // reshape the contents of the window keeping aspect ratio (disabled / malfunctioning) 220 | void my_reshape(int w, int h) 221 | { 222 | GLdouble size; 223 | GLdouble aspect; 224 | 225 | // use the whole window 226 | glViewport(0, 0, w, h); 227 | 228 | // 2-D orthographic drawing 229 | glMatrixMode(GL_PROJECTION); 230 | glLoadIdentity(); 231 | size = (GLdouble)((w >= h) ? w : h) / 2.0; 232 | if (w <= h) 233 | { 234 | aspect = (GLdouble)h/(GLdouble)w; 235 | glOrtho(-size, size, -size*aspect, size*aspect, -100000.0, 100000.0); 236 | } 237 | else 238 | { 239 | aspect = (GLdouble)w/(GLdouble)h; 240 | glOrtho(-size*aspect, size*aspect, -size, size, -100000.0, 100000.0); 241 | } 242 | 243 | // make the world and window coordinates coincide so that 1.0 in 244 | // model space equals one pixel in window space. 245 | glScaled(aspect, aspect, 1.0); 246 | 247 | // determine where to draw things 248 | glMatrixMode(GL_MODELVIEW); 249 | glLoadIdentity(); 250 | } 251 | 252 | // draw string in OpenGL at position x, y 253 | void draw_string(GLfloat x, GLfloat y, const char* word) 254 | { 255 | glColor4f(1.0, 1.0, 1.0, 0.0); 256 | glRasterPos2f(x, y); 257 | print_string(font_base, word); 258 | } 259 | -------------------------------------------------------------------------------- /src/client/keystroke.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/keystroke.h" 2 | #include "behavior_trees/glutkey.h" 3 | #include "behavior_trees/navigation.h" 4 | 5 | // keystrokes 6 | bool run = true; 7 | bool L_keypressed = false; 8 | bool L_keydown = false; 9 | bool R_keypressed = false; 10 | bool R_keydown = false; 11 | bool U_keypressed = false; 12 | bool U_keydown = false; 13 | bool D_keypressed = false; 14 | bool D_keydown = false; 15 | bool ENTER_keypressed = false; 16 | bool ENTER_keydown = false; 17 | bool BACK_keypressed = false; 18 | bool BACK_keydown = false; 19 | bool PAUSE_keypressed = false; 20 | bool PAUSE_keydown = false; 21 | bool ESC_keypressed = false; 22 | bool ESC_keydown = false; 23 | bool S1_keypressed = false; 24 | bool S1_keydown = false; 25 | bool S2_keypressed = false; 26 | bool S2_keydown = false; 27 | bool S3_keypressed = false; 28 | bool S3_keydown = false; 29 | 30 | extern NodeRoot root; 31 | extern bool* keyStates; 32 | 33 | int get_keypressed() 34 | { 35 | // check for left 36 | bool L_tmp = keyStates['a']; 37 | if( !L_keydown && L_tmp ) // key wasn't down previously, but now is down 38 | L_keypressed = true; // so it was just pressed 39 | else // otherwise, it was not just pressed 40 | L_keypressed = false; 41 | L_keydown = L_tmp; 42 | 43 | // check for right 44 | bool R_tmp = keyStates['e']; 45 | if( !R_keydown && R_tmp ) 46 | R_keypressed = true; 47 | else 48 | R_keypressed = false; 49 | R_keydown = R_tmp; 50 | 51 | // check for up 52 | bool U_tmp = keyStates['l']; 53 | if( !U_keydown && U_tmp ) 54 | U_keypressed = true; 55 | else 56 | U_keypressed = false; 57 | U_keydown = U_tmp; 58 | 59 | // check for down 60 | bool D_tmp = keyStates['r']; 61 | if( !D_keydown && D_tmp ) 62 | D_keypressed = true; 63 | else 64 | D_keypressed = false; 65 | D_keydown = D_tmp; 66 | 67 | // check for enter 68 | bool ENTER_tmp = keyStates[GLUT_KEY_ENTER]; 69 | if( !ENTER_keydown && ENTER_tmp ) 70 | ENTER_keypressed = true; 71 | else 72 | ENTER_keypressed = false; 73 | ENTER_keydown = ENTER_tmp; 74 | 75 | // check for backspace 76 | bool BACK_tmp = keyStates[GLUT_KEY_BACKSPACE]; 77 | if( !BACK_keydown && BACK_tmp ) 78 | BACK_keypressed = true; 79 | else 80 | BACK_keypressed = false; 81 | BACK_keydown = BACK_tmp; 82 | 83 | // check for spacebar 84 | bool PAUSE_tmp = keyStates[GLUT_KEY_SPACE]; 85 | if( !PAUSE_keydown && PAUSE_tmp ) 86 | PAUSE_keypressed = true; 87 | else 88 | PAUSE_keypressed = false; 89 | PAUSE_keydown = PAUSE_tmp; 90 | 91 | // check for exit 92 | bool ESC_tmp = keyStates[GLUT_KEY_ESC]; 93 | if( !ESC_keydown && ESC_tmp ) 94 | ESC_keypressed = true; 95 | else 96 | ESC_keypressed = false; 97 | ESC_keydown = ESC_tmp; 98 | 99 | // check for 1 (running) 100 | bool S1_tmp = keyStates[GLUT_KEY_1]; 101 | if( !S1_keydown && S1_tmp ) 102 | S1_keypressed = true; 103 | else 104 | S1_keypressed = false; 105 | S1_keydown = S1_tmp; 106 | 107 | // check for 2 (success) 108 | bool S2_tmp = keyStates[GLUT_KEY_2]; 109 | if( !S2_keydown && S2_tmp ) 110 | S2_keypressed = true; 111 | else 112 | S2_keypressed = false; 113 | S2_keydown = S2_tmp; 114 | 115 | // check for 3 (fail) 116 | bool S3_tmp = keyStates[GLUT_KEY_3]; 117 | if( !S3_keydown && S3_tmp ) 118 | S3_keypressed = true; 119 | else 120 | S3_keypressed = false; 121 | S3_keydown = S3_tmp; 122 | 123 | return 0; 124 | } 125 | 126 | int process_keypressed() 127 | { 128 | #ifdef DEBUG_MAIN 129 | if (L_keypressed) 130 | std::cout << "Left button pressed" << std::endl; 131 | if (R_keypressed) 132 | std::cout << "Right button pressed" << std::endl; 133 | if (U_keypressed) 134 | std::cout << "Up button pressed" << std::endl; 135 | if (D_keypressed) 136 | std::cout << "Down button pressed" << std::endl; 137 | if (ENTER_keypressed) 138 | std::cout << "Enter button pressed" << std::endl; 139 | if (BACK_keypressed) 140 | std::cout << "Backspace button pressed" << std::endl; 141 | if (PAUSE_keypressed) 142 | std::cout << "Spacebar button pressed" << std::endl; 143 | if (ESC_keypressed) 144 | std::cout << "Escape button pressed" << std::endl; 145 | #endif 146 | 147 | if (L_keypressed) 148 | navigate_left(); 149 | 150 | if (R_keypressed) 151 | navigate_right(); 152 | 153 | if (U_keypressed) 154 | navigate_up(); 155 | 156 | if (D_keypressed) 157 | navigate_down(); 158 | 159 | if (ENTER_keypressed) 160 | print_node_info(); 161 | 162 | if (BACK_keypressed) 163 | reset_overwritten(); 164 | 165 | if (PAUSE_keypressed) 166 | {} 167 | 168 | if (ESC_keypressed) 169 | run = false; 170 | 171 | // if (S1_keypressed) 172 | // set_node_state(FAILURE); 173 | 174 | // if (S2_keypressed) 175 | // set_node_state(SUCCESS); 176 | 177 | // if (S3_keypressed) 178 | // set_node_state(RUNNING); 179 | 180 | // if (S1_keypressed || S2_keypressed || S3_keypressed || BACK_keypressed) 181 | // root.execute(); 182 | 183 | return 0; 184 | } 185 | -------------------------------------------------------------------------------- /src/client/main.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #include "behavior_trees/parameters.h" 6 | #include "behavior_trees/node.h" 7 | #include "behavior_trees/parser.h" 8 | #include "behavior_trees/display.h" 9 | #include "behavior_trees/keystroke.h" 10 | #include "behavior_trees/navigation.h" 11 | #include "behavior_trees/robot_config.h" 12 | 13 | NodeRoot root; // the root of the bt 14 | Node *node_cursor = NULL; // used for displaying bt 15 | Node *node = NULL; // used for parsing bt 16 | extern bool run; 17 | std::string agent; 18 | 19 | void signal_callback_handler(int signum) 20 | { 21 | printf("Caught signal %d\n", signum); 22 | // Cleanup and close up stuff here 23 | // Terminate program 24 | exit(signum); 25 | } 26 | 27 | int main(int argc, char** argv) 28 | { 29 | std::cout << "Hello, world!" << std::endl; 30 | 31 | // setup signal interrupt handler 32 | signal(SIGINT, signal_callback_handler); 33 | 34 | // specify which options are available as cmd line arguments 35 | setupCmdLineReader(); 36 | 37 | // read agent id from command line parameters (--agent=mario) 38 | agent = readAgentFromCmdLine(argc, argv); 39 | 40 | // initialize the behavior tree client node 41 | ros::init(argc, argv, std::string("behavior_tree") + "_" + agent); 42 | 43 | // initialize OpenGL engine for visualization 44 | glut_setup(argc, argv); 45 | 46 | // point to the root of the behavior tree 47 | node_cursor = node = &root; 48 | node_cursor->set_highlighted(true); 49 | 50 | // create the bt from the file bt.txt (put on the path) 51 | std::cout << "----------------- PARSE FILE -----------------" << std::endl; 52 | parse_file(std::string("bt") + "_" + agent + ".txt"); 53 | 54 | // print the data parsed from the specification file 55 | std::cout << "----------------- PRINT TREE -----------------" << std::endl; 56 | root.print_subtree(); 57 | 58 | // wait until user inputs a key + Enter to start (space + Enter does not work) 59 | int key; 60 | std::cout << "Press Key To Start" << std::endl; 61 | std::cin >> key; 62 | 63 | // start ticking the root of the tree at frequency: TICK_FREQUENCY 64 | while (ros::ok()) 65 | { 66 | std::cout << "**** run" << run << std::endl; 67 | std::cout << "-------------- EXECUTE TREE --------------" << std::endl; 68 | root.execute(); // sending tick 69 | get_keypressed(); // processing keystrokes 70 | process_keypressed(); 71 | glut_process(); // update visualization 72 | glutPostRedisplay(); 73 | root.execute_reset_status(); 74 | ros::Duration(1.0/TICK_FREQUENCY).sleep(); 75 | std::cout << "**** run" << run << std::endl; 76 | } 77 | 78 | // missing to clear the dynamically allocated tree 79 | // delete_tree(); 80 | 81 | return 0; 82 | } 83 | -------------------------------------------------------------------------------- /src/client/navigation.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/navigation.h" 2 | 3 | extern Node *node_cursor; 4 | 5 | bool navigate_left() 6 | { 7 | if (node_cursor->get_prev_brother() != NULL) 8 | { 9 | node_cursor->set_highlighted(false); 10 | node_cursor = node_cursor->get_prev_brother(); 11 | node_cursor->set_highlighted(true); 12 | return true; 13 | } 14 | else 15 | { 16 | std::cout << '\a' << std::endl; 17 | return false; 18 | } 19 | } 20 | 21 | bool navigate_right() 22 | { 23 | if (node_cursor->get_next_brother() != NULL) 24 | { 25 | node_cursor->set_highlighted(false); 26 | node_cursor = node_cursor->get_next_brother(); 27 | node_cursor->set_highlighted(true); 28 | return true; 29 | } 30 | else 31 | { 32 | std::cout << '\a' << std::endl; 33 | return false; 34 | } 35 | } 36 | 37 | bool navigate_up() 38 | { 39 | if (node_cursor->get_parent() != NULL) 40 | { 41 | node_cursor->set_highlighted(false); 42 | node_cursor = node_cursor->get_parent(); 43 | node_cursor->set_highlighted(true); 44 | return true; 45 | } 46 | else 47 | { 48 | std::cout << '\a' << std::endl; 49 | return false; 50 | } 51 | } 52 | 53 | bool navigate_down() 54 | { 55 | if (node_cursor->get_first_child() != NULL) 56 | { 57 | node_cursor->set_highlighted(false); 58 | node_cursor = node_cursor->get_first_child(); 59 | node_cursor->set_highlighted(true); 60 | return true; 61 | } 62 | else 63 | { 64 | std::cout << '\a' << std::endl; 65 | return false; 66 | } 67 | } 68 | 69 | // this function needs to be checked 70 | void set_node_state(STATE state) 71 | { 72 | NODE_TYPE node_type = node_cursor->get_node_type(); 73 | if (node_type == ACTION) 74 | { 75 | if (state != NODE_ERROR) 76 | node_cursor->set_overwrite(state, true); 77 | else 78 | { 79 | std::cout << '\a' << std::endl; 80 | std::cout << "Action Result Selected Error" << std::endl; 81 | } 82 | } 83 | else if (node_type == CONDITION) 84 | { 85 | if (state != NODE_ERROR && state != RUNNING) 86 | node_cursor->set_overwrite(state, true); 87 | else 88 | { 89 | std::cout << '\a' << std::endl; 90 | std::cout << "Condition Result Selected Error" << std::endl; 91 | } 92 | } 93 | else 94 | { 95 | std::cout << '\a' << std::endl; 96 | std::cout << "Node Is Not Action, Cannot be Modified" << std::endl; 97 | } 98 | } 99 | 100 | void reset_overwritten() 101 | { 102 | node_cursor->set_overwrite(NODE_ERROR, false); 103 | } 104 | 105 | void reset_node_state() 106 | { 107 | node_cursor->set_overwrite(NODE_ERROR, false); 108 | } 109 | 110 | void print_node_info() 111 | { 112 | node_cursor->print_info(); 113 | } 114 | -------------------------------------------------------------------------------- /src/client/node.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | // #include 3 | 4 | #include "behavior_trees/node.h" 5 | #include "behavior_trees/display.h" 6 | #include "behavior_trees/parser.h" 7 | 8 | // constructor for the root node 9 | Node::Node() 10 | { 11 | depth_ = 0; 12 | number_children_ = 0; 13 | children_number_ = 0; 14 | highlighted_ = false; 15 | overwritten_ = false; 16 | overwritten_result_ = NODE_ERROR; 17 | node_status_ = NODE_ERROR; 18 | child_status_ = NODE_ERROR; 19 | first_child_ = NULL; 20 | curr_child_ = NULL; 21 | exec_child_ = NULL; 22 | next_brother_ = NULL; 23 | prev_brother_ = NULL; 24 | parent_ = NULL; 25 | } 26 | 27 | // constructor for any node but root 28 | Node::Node(Node* parent) 29 | { 30 | depth_ = parent->get_depth() + 1; 31 | number_children_ = 0; 32 | children_number_ = 0; 33 | highlighted_ = false; 34 | overwritten_ = false; 35 | overwritten_result_ = NODE_ERROR; 36 | node_status_ = NODE_ERROR; 37 | child_status_ = NODE_ERROR; 38 | first_child_ = NULL; 39 | curr_child_ = NULL; 40 | exec_child_ = NULL; 41 | next_brother_ = NULL; 42 | prev_brother_ = NULL; 43 | parent_ = parent; 44 | parent_->add_child(this); 45 | } 46 | 47 | // set node status to default and its children too (recursive) 48 | void Node::execute_reset_status() 49 | { 50 | node_status_ = NODE_ERROR; 51 | 52 | exec_child_ = first_child_; 53 | for (int i = 0; i < number_children_; i++) 54 | { 55 | ROS_INFO("child %d", i); 56 | // child_status_ = NODE_ERROR; // unnecessary 57 | exec_child_->execute_reset_status(); 58 | exec_child_ = exec_child_->next_brother_; 59 | } 60 | } 61 | 62 | // add child distinguishing whether it is the first one or not 63 | Node* Node::add_child(Node* my_child) 64 | { 65 | if (number_children_ == 0) 66 | { 67 | std::cout << "***adding first child***" << std::endl; 68 | first_child_ = curr_child_ = my_child; 69 | } 70 | else 71 | { 72 | std::cout << "***adding brother***" << std::endl; 73 | curr_child_ = curr_child_->add_brother(my_child, number_children_); 74 | } 75 | 76 | number_children_++; 77 | return curr_child_; 78 | } 79 | 80 | // if it is not the first child, it will be treated as a brother instead 81 | Node* Node::add_brother(Node* my_brother, int children_number) 82 | { 83 | next_brother_ = my_brother; 84 | next_brother_->set_prev_brother(this); 85 | 86 | next_brother_->set_children_number(this->get_children_number() + 1); 87 | return next_brother_; 88 | } 89 | 90 | // prints the depth, status, and number of children of a certain node 91 | void Node::print_info() 92 | { 93 | std::cout << "Depth: " << depth_ 94 | << " Status: " << node_status_ 95 | << " N. Children: " << number_children_ 96 | << std::endl; 97 | } 98 | 99 | // prints the information of the whole tree recursively calling itself 100 | void Node::print_subtree() 101 | { 102 | print_info(); 103 | if (number_children_ > 0) 104 | { 105 | first_child_->print_subtree(); 106 | } 107 | if (next_brother_ != NULL) 108 | { 109 | next_brother_->print_subtree(); 110 | } 111 | } 112 | 113 | // draws the tree in OpenGL trying to spread the nodes over the screen 114 | void Node::draw_subtree(GLfloat parent_x, GLfloat parent_y, 115 | int number_children, GLfloat parent_space) 116 | { 117 | GLfloat separation = parent_space / (number_children); 118 | 119 | // std::cout << "depth: " << depth_ << " separation: " << separation 120 | // << " parent space: " << parent_space 121 | // << " number of children: " << number_children << std::endl; 122 | 123 | GLfloat x = (GLfloat) ( 124 | parent_x + ( 125 | ((GLfloat)children_number_) - ((GLfloat)number_children - 1) / 2 126 | ) * separation); 127 | 128 | GLfloat y = (GLfloat) (parent_y - SPACE_HEIGHT); 129 | 130 | draw_node(x, y, this->get_node_type()); 131 | draw_connector(parent_x, parent_y, x, y); 132 | draw_status(x, y, node_status_); 133 | draw_string(x - CURSOR_WIDTH, y + 2.1 * CURSOR_WIDTH, 134 | get_node_name().c_str()); 135 | 136 | if (this->get_node_type() == ACTION) 137 | draw_string(x - CURSOR_WIDTH, y + 2.2 * NODE_WIDTH, 138 | get_ros_node_name().c_str()); 139 | 140 | if (highlighted_) 141 | draw_cursor(x, y); 142 | 143 | if (number_children_ > 0) 144 | { 145 | first_child_->draw_subtree(x, y, number_children_, separation); 146 | } 147 | if (next_brother_ != NULL) 148 | { 149 | next_brother_->draw_subtree(parent_x, parent_y, 150 | number_children, parent_space); 151 | } 152 | } 153 | 154 | /* -------------------------------------------------------------------------- */ 155 | /* ----------------------------Basic Nodes----------------------------------- */ 156 | /* -------------------------------------------------------------------------- */ 157 | 158 | NodeSelector::NodeSelector(Node* node) 159 | : Node(node) {} 160 | 161 | STATE NodeSelector::execute() 162 | { 163 | set_highlighted(true); 164 | std::cout << "Executing Selector" << std::endl; 165 | exec_child_ = first_child_; 166 | for (int i = 0; i < number_children_; i++) 167 | { 168 | std::cout << "ticking child: " << i << std::endl; 169 | child_status_ = exec_child_->execute(); 170 | set_highlighted(false); 171 | 172 | std::cout << "child status for comparison: " << child_status_ << std::endl; 173 | if (child_status_ == NODE_ERROR) 174 | return node_status_ = NODE_ERROR; 175 | else if (child_status_ == RUNNING) 176 | return node_status_ = RUNNING; 177 | else if (child_status_ == SUCCESS) 178 | return node_status_ = SUCCESS; 179 | 180 | std::cout << "pointing exec_child_ to next brother" << std::endl; 181 | exec_child_ = exec_child_->get_next_brother(); 182 | } 183 | return node_status_ = FAILURE; 184 | } 185 | 186 | NodeSequence::NodeSequence(Node* node) 187 | : Node(node) {} 188 | 189 | 190 | STATE NodeSequence::execute() 191 | { 192 | set_highlighted(true); 193 | std::cout << "Executing Sequence" << std::endl; 194 | exec_child_ = first_child_; 195 | for (int i = 0; i < number_children_; i++) 196 | { 197 | child_status_ = exec_child_->execute(); 198 | set_highlighted(false); 199 | if (child_status_ == NODE_ERROR) 200 | return node_status_ = NODE_ERROR; 201 | else if (child_status_ == RUNNING) 202 | return node_status_ = RUNNING; 203 | else if (child_status_ == FAILURE) 204 | return node_status_ = FAILURE; 205 | exec_child_ = exec_child_->get_next_brother(); 206 | } 207 | return node_status_ = SUCCESS; 208 | } 209 | 210 | NodeParallel::NodeParallel(Node* node) 211 | : Node(node) {} 212 | 213 | STATE NodeParallel::execute() 214 | { 215 | set_highlighted(true); 216 | int number_failure = 0; 217 | int number_success = 0; 218 | int number_error = 0; 219 | std::cout << "Executing Parallel" << std::endl; 220 | exec_child_ = first_child_; 221 | for (int i = 0; i < number_children_; i++) 222 | { 223 | child_status_ = exec_child_->execute(); 224 | set_highlighted(false); 225 | if (child_status_ == NODE_ERROR) 226 | number_error++; 227 | else if (child_status_ == FAILURE) 228 | number_failure++; 229 | else if (child_status_ == SUCCESS) 230 | number_success++; 231 | exec_child_ = exec_child_->get_next_brother(); 232 | } 233 | if (number_error > 0) 234 | return node_status_ = NODE_ERROR; 235 | else if (number_success >= number_children_/2) 236 | return node_status_ = SUCCESS; 237 | else if (number_failure >= number_children_/2) 238 | return node_status_ = FAILURE; 239 | else 240 | return node_status_ = RUNNING; 241 | } 242 | 243 | STATE NodeRoot::execute() 244 | { 245 | // there is no need to reset status because the nodes should 246 | // have a timeout to become NODE_ERROR after a while, if they 247 | // don't receive a feedback from the server. 248 | // execute_reset_status(); 249 | set_highlighted(true); 250 | std::cout << "---------- Executing Root ----------" << std::endl; 251 | return child_status_ = first_child_->execute(); 252 | set_highlighted(false); 253 | } 254 | 255 | /* -------------------------------------------------------------------------- */ 256 | /* ----------------------------Star Nodes------------------------------------ */ 257 | /* -------------------------------------------------------------------------- */ 258 | 259 | NodeSelectorStar::NodeSelectorStar(Node* node) 260 | : Node(node), 261 | current_running_child_(first_child_) {} 262 | 263 | STATE NodeSelectorStar::execute() 264 | { 265 | set_highlighted(true); 266 | std::cout << "Executing Selector Star" << std::endl; 267 | 268 | if (current_running_child_ == NULL) { 269 | current_running_child_ = first_child_; 270 | } 271 | 272 | exec_child_ = current_running_child_; 273 | 274 | do 275 | { 276 | child_status_ = exec_child_->execute(); 277 | set_highlighted(false); 278 | if (child_status_ == NODE_ERROR) 279 | { 280 | current_running_child_ = exec_child_; 281 | return node_status_ = NODE_ERROR; 282 | } 283 | else if (child_status_ == RUNNING) 284 | { 285 | current_running_child_ = exec_child_; 286 | return node_status_ = RUNNING; 287 | } 288 | else if (child_status_ == SUCCESS) 289 | { 290 | current_running_child_ = NULL; 291 | return node_status_ = SUCCESS; 292 | } 293 | exec_child_=exec_child_->get_next_brother(); 294 | } while(exec_child_ != NULL); 295 | 296 | current_running_child_ = NULL; 297 | return node_status_ = FAILURE; 298 | } 299 | 300 | NodeSequenceStar::NodeSequenceStar(Node* node) 301 | : Node(node), 302 | current_running_child_(first_child_) {} 303 | 304 | STATE NodeSequenceStar::execute() 305 | { 306 | set_highlighted(true); 307 | std::cout << "Executing Sequence Star" << std::endl; 308 | 309 | if (current_running_child_ == NULL) { 310 | current_running_child_ = first_child_; 311 | } 312 | 313 | exec_child_ = current_running_child_; 314 | 315 | 316 | do 317 | { 318 | child_status_ = exec_child_->execute(); 319 | set_highlighted(false); 320 | if (child_status_ == NODE_ERROR) 321 | { 322 | current_running_child_ = exec_child_; 323 | return node_status_ = NODE_ERROR; 324 | } 325 | else if (child_status_ == RUNNING) 326 | { 327 | current_running_child_ = exec_child_; 328 | return node_status_ = RUNNING; 329 | } 330 | else if (child_status_ == FAILURE) 331 | { 332 | current_running_child_ = NULL; 333 | return node_status_ = FAILURE; 334 | } 335 | exec_child_=exec_child_->get_next_brother(); 336 | } while (exec_child_ != NULL); 337 | 338 | current_running_child_ = NULL; 339 | return node_status_ = SUCCESS; 340 | } 341 | 342 | /* -------------------------------------------------------------------------- */ 343 | /* ------------------------------ROS Nodes----------------------------------- */ 344 | /* -------------------------------------------------------------------------- */ 345 | 346 | NodeROS::NodeROS(Node* node, std::string name) 347 | : Node(node), 348 | ros_node_name_(name), 349 | ac_(name, true) 350 | { 351 | std::cout << "ROS Client: " << ros_node_name_ << std::endl; 352 | ROS_INFO("Waiting for the corresponding actuator server to start."); 353 | ac_.waitForServer(); 354 | ROS_INFO("Actuator server started successfully."); 355 | received_ = false; 356 | finished_ = false; 357 | active_ = false; 358 | } 359 | 360 | void NodeROS::doneCb(const actionlib::SimpleClientGoalState& state, 361 | const behavior_trees::ROSResultConstPtr& result) 362 | { 363 | std::cout << "The Server is Finishing" << this << std::endl; 364 | // ROS_INFO("Finished in state [%s]", state.toString().c_str()); 365 | // ROS_INFO("Answer: %i", result->RESULT_); 366 | // { 367 | // boost::lock_guard lock(mutex_finished_); 368 | // finished_ = true; 369 | // } 370 | // received_ = true; 371 | } 372 | 373 | void NodeROS::activeCb() 374 | { 375 | std::cout << "active callback at Node: " << this << std::endl; 376 | ROS_INFO("Goal just went active"); 377 | { 378 | boost::lock_guard lock(mutex_active_); 379 | active_ = true; 380 | } 381 | } 382 | 383 | void NodeROS::feedbackCb(const behavior_trees::ROSFeedbackConstPtr& feedback) 384 | { 385 | std::cout << "Callback Feedback at Node: " << this << std::endl; 386 | ROS_INFO("Got Feedback status: %i", feedback->FEEDBACK_); 387 | // std::cout << "%%% cb var: " << node_status_ << std::endl; 388 | { 389 | boost::lock_guard lock(mutex_node_status_); 390 | node_status_ = (STATE) feedback->FEEDBACK_; 391 | } 392 | { 393 | boost::lock_guard lock(mutex_received_); 394 | received_ = true; 395 | } 396 | } 397 | 398 | STATE NodeROS::execute() 399 | { 400 | set_highlighted(true); 401 | glut_process(); 402 | std::cout << "NodeROS::execute()" << std::endl; 403 | if (overwritten_) 404 | { 405 | set_highlighted(false); 406 | return node_status_ = FAILURE; //overwritten_result_; 407 | } 408 | else 409 | { 410 | bool finished; 411 | // received_ = false; 412 | { 413 | boost::lock_guard lock(mutex_finished_); 414 | finished = finished_; 415 | } 416 | 417 | ROS_INFO("RECEIVED: %d", received_); 418 | if (!finished && !received_) 419 | { 420 | std::cout << "Sending Goal Client: " 421 | << ros_node_name_ << std::endl; 422 | behavior_trees::ROSGoal goal; 423 | goal.GOAL_ = 1; // possitive tick 424 | 425 | { 426 | boost::lock_guard lock(mutex_active_); 427 | active_ = false; 428 | } 429 | 430 | ac_.sendGoal(goal, 431 | boost::bind(&NodeROS::doneCb, this, _1, _2), 432 | boost::bind(&NodeROS::activeCb, this), 433 | boost::bind(&NodeROS::feedbackCb, this, _1)); 434 | 435 | std::cout << "Waiting for Feedback at Node: " << this << std::endl; 436 | while (!received_ && !active_) 437 | { 438 | sleep(0.01); 439 | // std::cout << "*"; 440 | } 441 | std::cout << "Received Feedback at Node: " << this << std::endl; 442 | } 443 | { 444 | boost::lock_guard lock(mutex_received_); 445 | received_ = false; 446 | } 447 | { 448 | boost::lock_guard lock(mutex_node_status_); 449 | std::cout << "STATUS: " << node_status_ << std::endl; 450 | set_highlighted(false); 451 | return node_status_; 452 | } 453 | } 454 | } 455 | 456 | NodeCondition::NodeCondition(Node* node, std::string varlabel, 457 | std::string relation, std::string constant) 458 | : Node(node), varlabel_(varlabel), relation_(relation), constant_(constant) 459 | {} 460 | 461 | STATE NodeCondition::execute() 462 | { 463 | set_highlighted(true); 464 | std::cout << "Executing Condition" << std::endl; 465 | 466 | if (relation_.compare("=")) 467 | std::cout << "it's equality" << std::endl; 468 | 469 | unsigned int idx = 0; 470 | for (std::vector::iterator it = global_varname.begin(); 471 | it != global_varname.end(); ++it) 472 | { 473 | if (*it == varlabel_) 474 | { 475 | std::cout << "found match " << idx << std::endl; 476 | break; 477 | } 478 | idx++; 479 | } 480 | double val = global_varvalue.at(idx); 481 | std::cout << "val" << val << std::endl; 482 | 483 | set_highlighted(false); 484 | switch (relation_.at(0)) 485 | { 486 | case '=': return node_status_ = (val == std::stod(constant_))? 487 | SUCCESS : FAILURE; break; 488 | case '>': return node_status_ = (val >= std::stod(constant_))? 489 | SUCCESS : FAILURE; break; 490 | case '<': return node_status_ = (val <= std::stod(constant_))? 491 | SUCCESS : FAILURE; break; 492 | default: std::cout << "relation not implemented (choose =, >, <)" 493 | << std::endl; break; 494 | } 495 | return node_status_ = NODE_ERROR; 496 | } 497 | 498 | NodeDecorator::NodeDecorator(Node* node, std::string next_state, 499 | std::string curr_state, std::string prev_status) 500 | : Node(node), next_state_(next_state), curr_state_(curr_state), 501 | prev_status_(prev_status) 502 | {} 503 | 504 | STATE NodeDecorator::execute() 505 | { 506 | set_highlighted(true); 507 | std::cout << "Executing Decorator" << std::endl; 508 | 509 | exec_child_ = first_child_; 510 | child_status_ = exec_child_->execute(); 511 | set_highlighted(false); 512 | 513 | if (child_status_ == SUCCESS) 514 | { 515 | unsigned int idx = 0; 516 | for (std::vector::iterator it = global_varname.begin(); 517 | it != global_varname.end(); ++it) 518 | { 519 | if (*it == prev_status_) 520 | break; 521 | idx++; 522 | } 523 | // prev_status = success 524 | global_varvalue[idx] = 0; 525 | std::cout << "global_varvalue" << global_varvalue[idx] << std::endl; 526 | 527 | idx = 0; 528 | for (std::vector::iterator it = global_varname.begin(); 529 | it != global_varname.end(); ++it) 530 | { 531 | if (*it == curr_state_) 532 | break; 533 | idx++; 534 | } 535 | // curr_state = next_state 536 | global_varvalue[idx] = std::stod(next_state_); 537 | std::cout << "global_varvalue" << global_varvalue[idx] << std::endl; 538 | 539 | return node_status_ = SUCCESS; 540 | } 541 | else if (child_status_ == FAILURE) 542 | { 543 | unsigned int idx = 0; 544 | for (std::vector::iterator it = global_varname.begin(); 545 | it != global_varname.end(); ++it) 546 | { 547 | if (*it == prev_status_) 548 | break; 549 | idx++; 550 | } 551 | // prev_status = failure 552 | global_varvalue[idx] = 1; 553 | std::cout << "global_varvalue" << global_varvalue[idx] << std::endl; 554 | 555 | idx = 0; 556 | for (std::vector::iterator it = global_varname.begin(); 557 | it != global_varname.end(); ++it) 558 | { 559 | if (*it == curr_state_) 560 | break; 561 | idx++; 562 | } 563 | // curr_state = next_state 564 | global_varvalue[idx] = std::stod(next_state_); 565 | std::cout << "global_varvalue" << global_varvalue[idx] << std::endl; 566 | 567 | return node_status_ = SUCCESS; 568 | } 569 | else if (child_status_ == RUNNING) 570 | return node_status_ = RUNNING; 571 | else 572 | return node_status_ = NODE_ERROR; 573 | } 574 | 575 | 576 | // enum STATE 577 | // { 578 | // FAILURE = 0, 579 | // SUCCESS = 1, 580 | // RUNNING = 2, 581 | // NODE_ERROR = 3 582 | // }; 583 | -------------------------------------------------------------------------------- /src/client/parser.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/parser.h" 2 | #include "behavior_trees/node.h" 3 | 4 | #include 5 | #include 6 | 7 | extern std::string agent; 8 | extern Node *node; 9 | bool action_detected = false; 10 | bool condition_detected = false; 11 | bool variable_detected = false; 12 | bool decorator_detected = false; 13 | 14 | std::vector global_varname; 15 | std::vector global_varvalue; 16 | 17 | int process_substring(std::string sub) 18 | { 19 | bool star_detected = false; 20 | if (sub.length() == 2) 21 | { 22 | if (sub.at(1) == '*') 23 | { 24 | star_detected = true; 25 | sub = sub.at(0); 26 | } 27 | } 28 | switch ( *(sub.c_str()) ) 29 | { 30 | case '{': std::cout << "Open Brace" << std::endl; 31 | if (!star_detected) node = new NodeSelector(node); 32 | else node = new NodeSelectorStar(node); 33 | break; 34 | case '}': std::cout << "Close Brace" << std::endl; 35 | node = node->get_parent(); break; 36 | case '[': std::cout << "Open Bracket" << std::endl; 37 | if (!star_detected) node = new NodeSequence(node); 38 | else node = new NodeSequenceStar(node); 39 | break; 40 | case ']': std::cout << "Close Bracket" << std::endl; 41 | node = node->get_parent(); break; 42 | case '/': std::cout << "Open Slash" << std::endl; 43 | node = new NodeParallel(node); break; 44 | case '|': std::cout << "Close Slash" << std::endl; 45 | node = node->get_parent(); break; 46 | case 'A': std::cout << "ROS Action Detected" << std::endl; 47 | action_detected = true; break; 48 | case 'C': std::cout << "Condition Detected" << std::endl; 49 | condition_detected = true; break; 50 | case 'V': std::cout << "Variable Detected" << std::endl; 51 | variable_detected = true; break; 52 | case 'd': std::cout << "Decorator Open Detected" << std::endl; 53 | decorator_detected = true; break; 54 | case 'D': std::cout << "Decorator Close Detected" << std::endl; 55 | node = node->get_parent(); break; 56 | 57 | default: break; 58 | } 59 | return 0; 60 | } 61 | 62 | int parse_file(std::string name) 63 | { 64 | std::string line; 65 | std::string path = ros::package::getPath("behavior_trees"); 66 | std::ifstream file(path + "/data/" + name); 67 | 68 | if (!file.is_open()) 69 | { 70 | std::cout << "Couldn't Open File" << std::endl; 71 | return 1; 72 | } 73 | std::cout << "File Called " << name << " Opened Correctly" << std::endl; 74 | 75 | while ( file.good() ) 76 | { 77 | getline (file, line); 78 | std::cout << "Reading line: " << line << std::endl; 79 | std::istringstream iss(line); 80 | int i = 0; 81 | do 82 | { 83 | std::string sub; 84 | iss >> sub; 85 | std::cout << "Substring: " << " i: " << i 86 | << " n: " << sub << std::endl; 87 | process_substring(sub); 88 | if (action_detected) 89 | { 90 | std::string actionname; 91 | iss >> actionname; 92 | std::cout << "ROS Action Detected: " << actionname << std::endl; 93 | node = new NodeROS(node, actionname + "_" + agent); 94 | node = node->get_parent(); 95 | action_detected = false; 96 | } 97 | if (condition_detected) 98 | { 99 | std::string varlabel; 100 | std::string relation; 101 | std::string constant; 102 | iss >> varlabel; 103 | iss >> relation; 104 | iss >> constant; 105 | std::cout << "Condition Detected: " 106 | << varlabel << relation << constant << std::endl; 107 | node = new NodeCondition(node, varlabel, relation, constant); 108 | node = node->get_parent(); 109 | condition_detected = false; 110 | } 111 | if (variable_detected) 112 | { 113 | std::string varlabel; 114 | std::string initialval; 115 | iss >> varlabel; 116 | iss >> initialval; 117 | std::cout << "Variable Detected: " 118 | << varlabel << initialval << std::endl; 119 | global_varname.push_back(varlabel); 120 | global_varvalue.push_back(std::stod(initialval)); 121 | std::cout << "Printing variable labels" << std::endl; 122 | std::copy(global_varname.begin(), global_varname.end(), 123 | std::ostream_iterator(std::cout, " ")); 124 | std::cout << std::endl; 125 | std::cout << "Printing variable values" << std::endl; 126 | std::copy(global_varvalue.begin(), global_varvalue.end(), 127 | std::ostream_iterator(std::cout, " ")); 128 | std::cout << std::endl; 129 | variable_detected = false; 130 | } 131 | if (decorator_detected) 132 | { 133 | std::string next_state; 134 | std::string curr_state; 135 | std::string prev_status; 136 | iss >> next_state; 137 | iss >> curr_state; 138 | iss >> prev_status; 139 | std::cout << "Decorator Detected: " 140 | << next_state << prev_status << std::endl; 141 | node = new NodeDecorator(node, next_state, curr_state, 142 | prev_status); 143 | decorator_detected = false; 144 | } 145 | i++; 146 | } while (iss); 147 | } 148 | file.close(); 149 | return 0; 150 | } 151 | -------------------------------------------------------------------------------- /src/client/robot_config.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | 4 | po::options_description desc("Allowed options"); 5 | 6 | void setupCmdLineReader() 7 | { 8 | addCmdLineOption("agent"); 9 | } 10 | 11 | void addCmdLineOption(std::string argumentName) 12 | { 13 | desc.add_options() 14 | (argumentName.c_str(), po::value(), ("Custom" + argumentName).c_str()); 15 | } 16 | 17 | std::string readCmdLineOption(int argc, char** argv, std::string argument_name) 18 | { 19 | po::variables_map vm; 20 | po::store(po::parse_command_line(argc, argv, desc), vm); 21 | po::notify(vm); 22 | std::string return_string = ""; 23 | if (vm.count(argument_name)) 24 | { 25 | return_string = vm[argument_name].as(); 26 | // std::cout << argument_name << " was set to " << return_string << std::endl; 27 | } 28 | else 29 | { 30 | // std::cout << argument_name << " was not set" << std::endl;; 31 | } 32 | return return_string; 33 | } 34 | 35 | // std::string readRobotIPFromCmdLine(int argc, char** argv) 36 | // { 37 | // std::string robot_ip = readCmdLineOption(argc, argv, "robot_ip"); 38 | // if (robot_ip.length() < 1) 39 | // { 40 | // robot_ip = "localhost"; 41 | // } 42 | // return robot_ip; 43 | // } 44 | 45 | // std::string readColorFromCmdLine(int argc, char** argv) 46 | // { 47 | // std::string color_string = readCmdLineOption(argc, argv, "color"); 48 | // if (color_string.length() < 1) 49 | // { 50 | // color_string = "red"; 51 | // } 52 | // return color_string; 53 | // } 54 | 55 | std::string readAgentFromCmdLine(int argc, char** argv) 56 | { 57 | std::string agent_string = readCmdLineOption(argc, argv, "agent"); 58 | if (agent_string.length() < 1) 59 | { 60 | agent_string = "default"; 61 | } 62 | return agent_string; 63 | } 64 | -------------------------------------------------------------------------------- /src/client/rosaction.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/rosaction.h" // contains ROSAction class to implement servers 2 | #include // automatically generated actionlib header 3 | #include 4 | 5 | bool busy = false; 6 | // construct action with: A name 7 | ROSAction::ROSAction(std::string name) : 8 | as_(nh_, name, false), 9 | action_name_(name), 10 | started_(false), 11 | active_(false), 12 | start_time_(ros::Time::now()), 13 | elapsed_time_((ros::Duration) 0) 14 | { 15 | feedback_.FEEDBACK_ = NODE_ERROR; 16 | result_.RESULT_ = NODE_ERROR; 17 | as_.registerGoalCallback(boost::bind(&ROSAction::goalCB, this)); 18 | as_.registerPreemptCallback(boost::bind(&ROSAction::preemptCB, this)); 19 | as_.start(); 20 | } 21 | 22 | // destroy action, i.e. default thread 23 | ROSAction::~ROSAction(void) 24 | {} 25 | 26 | // thread that calls executeCB 27 | void ROSAction::executionThread() 28 | { 29 | ros::Rate r(EXECUTION_FREQUENCY); 30 | ros::Time t0 = start_time_ = ros::Time::now(); 31 | 32 | // while (as_.isPreemptRequested() || ros::ok()) 33 | while (is_active() && ros::ok()) 34 | { 35 | std::cout << "execution_thread_.get_id()" << execution_thread_.get_id() << std::endl; 36 | 37 | bool active = timeout_check(); // check if tick was received 38 | std::cout << "im active: " << active << std::endl; 39 | { 40 | boost::lock_guard lock(mutex_active_); 41 | active_ = active = active ? active_ : false; 42 | } 43 | if (active) 44 | { 45 | ros::Duration dt = ros::Time::now() - t0; 46 | t0 = ros::Time::now(); 47 | std::cout << "executing cb" << dt << std::endl; 48 | if (executeCB(dt)) // execute user personal code if finished exit fast 49 | { 50 | std::cout << "callback returned!" << std::endl; 51 | break; 52 | } 53 | } 54 | else 55 | { 56 | std::cout << "executionThread not active" << std::endl; 57 | } 58 | r.sleep(); // wait to match frequency 59 | } 60 | std::cout << "About to Destroy Thread" << std::endl; 61 | 62 | std::cout << "----------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------------" << std::endl; 63 | // set_feedback(NODE_ERROR); 64 | send_feedback(); 65 | stop(); 66 | } 67 | 68 | // called each time a goal is received 69 | void ROSAction::goalCB() 70 | { 71 | std::cout << "****************************%%%%%%%%%% goalCB %%%%%%%%%%" << std::endl; 72 | // could add conditions to accept goal or not 73 | goal_ = as_.acceptNewGoal()->GOAL_; 74 | std::cout << "Received Goal: " << goal_ << std::endl; 75 | 76 | // send_feedback(); 77 | 78 | // if (!busy) 79 | // { 80 | // feedback_.FEEDBACK_ = NODE_ERROR; 81 | // result_.RESULT_ = NODE_ERROR; 82 | // } 83 | 84 | if (feedback_.FEEDBACK_ != SUCCESS && 85 | feedback_.FEEDBACK_ != FAILURE) 86 | { 87 | bool started; // is thread running? 88 | { 89 | boost::lock_guard lock(mutex_started_); 90 | started = started_; 91 | } 92 | std::cout << "started: " << started << std::endl; 93 | if (started) 94 | { 95 | if (goal_ > 0) // possitive tick 96 | reset_timeout(); 97 | else if (goal_ < 0) // negative tick 98 | { 99 | std::cout << "Got a negative tick" << std::endl; 100 | stop(); 101 | } 102 | else // neutral tick 103 | {} 104 | } 105 | else 106 | { 107 | if (goal_ > 0) // possitive tick 108 | start(); 109 | else if (goal_ < 0) // negative tick 110 | {} 111 | else // neutral tick 112 | {} 113 | } 114 | } 115 | 116 | if (feedback_.FEEDBACK_ == SUCCESS || 117 | feedback_.FEEDBACK_ == FAILURE) 118 | { 119 | boost::lock_guard lock_b(mutex_feedback_); 120 | boost::lock_guard lock_c(mutex_result_); 121 | feedback_.FEEDBACK_ = NODE_ERROR; 122 | result_.RESULT_ = NODE_ERROR; 123 | } 124 | 125 | std::cout << "****************************%%%%%%%%%% goalCB Exit%%%%%%%%%%" << std::endl; 126 | } 127 | 128 | // called each time a goal is preempted 129 | void ROSAction::preemptCB() 130 | { 131 | std::cout << "%%%%%%%%%% preemptCB %%%%%%%%%%" << std::endl; 132 | reset_timeout(); 133 | // as_.setPreempted(); 134 | } 135 | 136 | // start thread 137 | void ROSAction::start() 138 | { 139 | std::cout << "Executing Reset Callback Now" << std::endl; 140 | resetCB(); 141 | { 142 | boost::lock_guard lock(mutex_started_); 143 | started_ = true; 144 | } 145 | { 146 | boost::lock_guard lock(mutex_active_); 147 | active_ = true; 148 | } 149 | std::cout << "Starting Thread Now" << std::endl; 150 | std::cout << "++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++++" << std::endl; 151 | 152 | execution_thread_ = boost::thread( 153 | boost::bind(&ROSAction::executionThread, this) ); 154 | } 155 | 156 | // stop thread 157 | void ROSAction::stop() 158 | { 159 | std::cout << "Stopping Thread Now" << std::endl; 160 | { 161 | boost::lock_guard lock(mutex_started_); 162 | started_ = false; 163 | } 164 | { 165 | boost::lock_guard lock(mutex_active_); 166 | active_ = false; 167 | } 168 | std::cout << "Stopped successfully" << std::endl; 169 | // execution_thread_.join(); 170 | } 171 | 172 | // activate executeCB 173 | void ROSAction::activate() 174 | { 175 | std::cout << "Activating Thread Now" << std::endl; 176 | boost::lock_guard lock(mutex_active_); 177 | active_ = true; 178 | } 179 | 180 | // deactivate executeCB 181 | void ROSAction::deactivate() 182 | { 183 | std::cout << "Deactivating Thread Now" << std::endl; 184 | boost::lock_guard lock(mutex_active_); 185 | active_ = false; 186 | } 187 | 188 | // get status of started 189 | bool ROSAction::is_started() 190 | { 191 | boost::lock_guard lock(mutex_started_); 192 | return started_; 193 | } 194 | 195 | // get status of active 196 | bool ROSAction::is_active() 197 | { 198 | boost::lock_guard lock(mutex_active_); 199 | return active_; 200 | } 201 | 202 | // check if no tick was received is TIMEOUT_THRESHOLD 203 | bool ROSAction::timeout_check() // called from executionThread 204 | { 205 | ros::Duration dt = (ros::Duration) 0; 206 | { 207 | boost::lock_guard lock_a(mutex_start_time_); 208 | boost::lock_guard lock_b(mutex_elapsed_time_); 209 | dt = elapsed_time_ = ros::Time::now() - start_time_; 210 | } 211 | std::cout << "elapsed_time since tick: " << dt.toSec() << std::endl; 212 | return dt.toSec() > TIMEOUT_THRESHOLD ? false : true; 213 | } 214 | 215 | // reset because tick was received possibly at TICK_FREQUENCY 216 | void ROSAction::reset_timeout() // called from goalCB 217 | { 218 | boost::lock_guard lock(mutex_start_time_); 219 | start_time_ = ros::Time::now(); 220 | } 221 | 222 | // send partial feedback 223 | void ROSAction::send_feedback() 224 | { 225 | std::cout << "Sending Feedback now" << std::endl; 226 | boost::lock_guard lock(mutex_feedback_); 227 | std::cout << "Sending Feedback: " << feedback_.FEEDBACK_ << std::endl; 228 | // feedback_.FEEDBACK_ = state; 229 | as_.publishFeedback(feedback_); 230 | } 231 | 232 | // send final result 233 | void ROSAction::send_result() 234 | { 235 | std::cout << "Sending Result now" << std::endl; 236 | boost::lock_guard lock(mutex_result_); 237 | // result_.RESULT_ = state; 238 | as_.setSucceeded(result_); 239 | } 240 | 241 | // sets the feedback to a certain value before sent in callback 242 | void ROSAction::set_feedback(STATE state) 243 | { 244 | boost::lock_guard lock(mutex_feedback_); 245 | feedback_.FEEDBACK_ = state; 246 | } 247 | 248 | // sets the result to a certain value before sent in callback 249 | void ROSAction::set_result(STATE state) 250 | { 251 | boost::lock_guard lock(mutex_result_); 252 | result_.RESULT_ = state; 253 | } 254 | -------------------------------------------------------------------------------- /src/server/template_server.cpp: -------------------------------------------------------------------------------- 1 | #include "behavior_trees/rosaction.h" 2 | #include "behavior_trees/robot_config.h" 3 | 4 | // simple template to implement actions and conditions for the 5 | // platform being used, the execute callback is going to be 6 | // executed with frenquency: EXECUTION_FREQUENCY 7 | 8 | // class definition overriding the functions executeCB and resetCB 9 | class ActionName : ROSAction 10 | { 11 | public: 12 | ActionName(std::string name) : 13 | ROSAction(name), 14 | time_to_complete_((ros::Duration) 0) // used for this example 15 | {} 16 | 17 | // the action succeeds automatically after 5 seconds 18 | int executeCB(ros::Duration dt) 19 | { 20 | std::cout << "**ActionName -%- Executing Main Task, elapsed_time: " 21 | << dt.toSec() << std::endl; 22 | std::cout << "**ActionName -%- elapsed_time: " 23 | << time_to_complete_.toSec() << std::endl; 24 | 25 | time_to_complete_ += dt; 26 | 27 | if (time_to_complete_.toSec() < 5) 28 | { 29 | set_feedback(RUNNING); 30 | // feedback_.FEEDBACK_ = RUNNING; 31 | // as_.publishFeedback(feedback_); 32 | return 0; // 'allows' this thread to continue running 33 | } 34 | else if (time_to_complete_.toSec() >= 5) 35 | { 36 | set_feedback(SUCCESS); 37 | // set_feedback(FAILURE); 38 | // feedback_.FEEDBACK_ = FAILURE; 39 | // as_.publishFeedback(feedback_); 40 | // result_.RESULT_ = FAILURE; 41 | // as_.setSucceeded(result_); 42 | // stop(); // stop allows new threads to be created 43 | return 1; // 'forces' this thread to finish securely 44 | } 45 | return 0; 46 | } 47 | 48 | void resetCB() 49 | { 50 | time_to_complete_ = (ros::Duration) 0; 51 | } 52 | 53 | // member variables 54 | ros::Duration time_to_complete_; 55 | }; 56 | 57 | int main(int argc, char** argv) 58 | { 59 | std::cout << "Hello, world!" << std::endl; 60 | // specify which options are available as cmd line arguments 61 | setupCmdLineReader(); 62 | // read agent id from command line parameters (--agent=mario) 63 | std::string agent = readAgentFromCmdLine(argc, argv); 64 | // initialize the behavior tree server node 65 | ros::init(argc, argv, std::string("action_name") + "_" + agent); // name used for bt.txt 66 | ActionName server(ros::this_node::getName()); 67 | ros::spin(); 68 | return 0; 69 | } 70 | --------------------------------------------------------------------------------