├── .gitignore ├── LICENSE.md ├── Makefile ├── README.md ├── controller ├── __init__.py ├── abs │ ├── robot_wrapper.py │ └── skill_item.py ├── assets │ ├── Roboto-Medium.ttf │ ├── gear │ │ └── model.pth │ ├── minispec_syntax.txt │ └── tello │ │ ├── guides.txt │ │ ├── high_level_skills copy.json │ │ ├── high_level_skills.json │ │ ├── plan_examples.txt │ │ ├── plan_examples_back_up.txt │ │ ├── prompt_plan.txt │ │ └── prompt_probe.txt ├── gear_wrapper.py ├── llm_controller.py ├── llm_planner.py ├── llm_wrapper.py ├── minispec_interpreter.py ├── shared_frame.py ├── skillset.py ├── tello_wrapper.py ├── utils.py ├── virtual_robot_wrapper.py ├── vision_skill_wrapper.py ├── yolo_client.py └── yolo_grpc_client.py ├── docker ├── env.list ├── router │ └── Dockerfile └── yolo │ └── Dockerfile ├── proto ├── generate.sh ├── generated │ ├── README.md │ └── __init__.py └── hyrch_serving.proto ├── serving ├── router │ ├── router.py │ └── service_manager.py ├── webui │ ├── drone-pov.html │ ├── header.html │ ├── install_requirements.sh │ └── typefly.py └── yolo │ └── yolo_service.py └── test ├── aiohttp-client.py ├── gpt-latency-measurement.py ├── images └── kitchen.webp ├── interpreter-test.py ├── stop-tello.py ├── tello-test.py ├── yolo-grpc-test.py ├── yolo-test-raw.py └── yolo-test.py /.gitignore: -------------------------------------------------------------------------------- 1 | # Byte-compiled / optimized / DLL files 2 | __pycache__/ 3 | *.py[cod] 4 | *$py.class 5 | 6 | # C extensions 7 | *.so 8 | 9 | # Distribution / packaging 10 | .Python 11 | build/ 12 | develop-eggs/ 13 | dist/ 14 | downloads/ 15 | eggs/ 16 | .eggs/ 17 | lib/ 18 | lib64/ 19 | parts/ 20 | sdist/ 21 | var/ 22 | wheels/ 23 | share/python-wheels/ 24 | *.egg-info/ 25 | .installed.cfg 26 | *.egg 27 | MANIFEST 28 | 29 | # PyInstaller 30 | # Usually these files are written by a python script from a template 31 | # before PyInstaller builds the exe, so as to inject date/other infos into it. 32 | *.manifest 33 | *.spec 34 | 35 | # Installer logs 36 | pip-log.txt 37 | pip-delete-this-directory.txt 38 | 39 | # Unit test / coverage reports 40 | htmlcov/ 41 | .tox/ 42 | .nox/ 43 | .coverage 44 | .coverage.* 45 | .cache 46 | nosetests.xml 47 | coverage.xml 48 | *.cover 49 | *.py,cover 50 | .hypothesis/ 51 | .pytest_cache/ 52 | cover/ 53 | 54 | # Translations 55 | *.mo 56 | *.pot 57 | 58 | # Django stuff: 59 | *.log 60 | local_settings.py 61 | db.sqlite3 62 | db.sqlite3-journal 63 | 64 | # Flask stuff: 65 | instance/ 66 | .webassets-cache 67 | 68 | # Scrapy stuff: 69 | .scrapy 70 | 71 | # Sphinx documentation 72 | docs/_build/ 73 | 74 | # PyBuilder 75 | .pybuilder/ 76 | target/ 77 | 78 | # Jupyter Notebook 79 | .ipynb_checkpoints 80 | 81 | # IPython 82 | profile_default/ 83 | ipython_config.py 84 | 85 | # pyenv 86 | # For a library or package, you might want to ignore these files since the code is 87 | # intended to run in multiple environments; otherwise, check them in: 88 | # .python-version 89 | 90 | # pipenv 91 | # According to pypa/pipenv#598, it is recommended to include Pipfile.lock in version control. 92 | # However, in case of collaboration, if having platform-specific dependencies or dependencies 93 | # having no cross-platform support, pipenv may install dependencies that don't work, or not 94 | # install all needed dependencies. 95 | #Pipfile.lock 96 | 97 | # poetry 98 | # Similar to Pipfile.lock, it is generally recommended to include poetry.lock in version control. 99 | # This is especially recommended for binary packages to ensure reproducibility, and is more 100 | # commonly ignored for libraries. 101 | # https://python-poetry.org/docs/basic-usage/#commit-your-poetrylock-file-to-version-control 102 | #poetry.lock 103 | 104 | # pdm 105 | # Similar to Pipfile.lock, it is generally recommended to include pdm.lock in version control. 106 | #pdm.lock 107 | # pdm stores project-wide configurations in .pdm.toml, but it is recommended to not include it 108 | # in version control. 109 | # https://pdm.fming.dev/#use-with-ide 110 | .pdm.toml 111 | 112 | # PEP 582; used by e.g. github.com/David-OConnor/pyflow and github.com/pdm-project/pdm 113 | __pypackages__/ 114 | 115 | # Celery stuff 116 | celerybeat-schedule 117 | celerybeat.pid 118 | 119 | # SageMath parsed files 120 | *.sage.py 121 | 122 | # Environments 123 | .env 124 | .venv 125 | env/ 126 | venv/ 127 | ENV/ 128 | env.bak/ 129 | venv.bak/ 130 | 131 | # Spyder project settings 132 | .spyderproject 133 | .spyproject 134 | 135 | # Rope project settings 136 | .ropeproject 137 | 138 | # mkdocs documentation 139 | /site 140 | 141 | # mypy 142 | .mypy_cache/ 143 | .dmypy.json 144 | dmypy.json 145 | 146 | # Pyre type checker 147 | .pyre/ 148 | 149 | # pytype static type analyzer 150 | .pytype/ 151 | 152 | # Cython debug symbols 153 | cython_debug/ 154 | 155 | # PyCharm 156 | # JetBrains specific template is maintained in a separate JetBrains.gitignore that can 157 | # be found at https://github.com/github/gitignore/blob/main/Global/JetBrains.gitignore 158 | # and can be added to the global gitignore or merged into this file. For a more nuclear 159 | # option (not recommended) you can uncomment the following to ignore the entire idea folder. 160 | #.idea/ 161 | 162 | *.pt 163 | .vscode/ 164 | images/ 165 | .DS_Store 166 | proto/generated/*.py 167 | test.py 168 | cache/ 169 | chat_log.txt 170 | results/ -------------------------------------------------------------------------------- /LICENSE.md: -------------------------------------------------------------------------------- 1 | GNU GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | Preamble 9 | 10 | The GNU General Public License is a free, copyleft license for 11 | software and other kinds of works. 12 | 13 | The licenses for most software and other practical works are designed 14 | to take away your freedom to share and change the works. By contrast, 15 | the GNU General Public License is intended to guarantee your freedom to 16 | share and change all versions of a program--to make sure it remains free 17 | software for all its users. We, the Free Software Foundation, use the 18 | GNU General Public License for most of our software; it applies also to 19 | any other work released this way by its authors. You can apply it to 20 | your programs, too. 21 | 22 | When we speak of free software, we are referring to freedom, not 23 | price. Our General Public Licenses are designed to make sure that you 24 | have the freedom to distribute copies of free software (and charge for 25 | them if you wish), that you receive source code or can get it if you 26 | want it, that you can change the software or use pieces of it in new 27 | free programs, and that you know you can do these things. 28 | 29 | To protect your rights, we need to prevent others from denying you 30 | these rights or asking you to surrender the rights. Therefore, you have 31 | certain responsibilities if you distribute copies of the software, or if 32 | you modify it: responsibilities to respect the freedom of others. 33 | 34 | For example, if you distribute copies of such a program, whether 35 | gratis or for a fee, you must pass on to the recipients the same 36 | freedoms that you received. You must make sure that they, too, receive 37 | or can get the source code. And you must show them these terms so they 38 | know their rights. 39 | 40 | Developers that use the GNU GPL protect your rights with two steps: 41 | (1) assert copyright on the software, and (2) offer you this License 42 | giving you legal permission to copy, distribute and/or modify it. 43 | 44 | For the developers' and authors' protection, the GPL clearly explains 45 | that there is no warranty for this free software. For both users' and 46 | authors' sake, the GPL requires that modified versions be marked as 47 | changed, so that their problems will not be attributed erroneously to 48 | authors of previous versions. 49 | 50 | Some devices are designed to deny users access to install or run 51 | modified versions of the software inside them, although the manufacturer 52 | can do so. This is fundamentally incompatible with the aim of 53 | protecting users' freedom to change the software. The systematic 54 | pattern of such abuse occurs in the area of products for individuals to 55 | use, which is precisely where it is most unacceptable. Therefore, we 56 | have designed this version of the GPL to prohibit the practice for those 57 | products. If such problems arise substantially in other domains, we 58 | stand ready to extend this provision to those domains in future versions 59 | of the GPL, as needed to protect the freedom of users. 60 | 61 | Finally, every program is threatened constantly by software patents. 62 | States should not allow patents to restrict development and use of 63 | software on general-purpose computers, but in those that do, we wish to 64 | avoid the special danger that patents applied to a free program could 65 | make it effectively proprietary. To prevent this, the GPL assures that 66 | patents cannot be used to render the program non-free. 67 | 68 | The precise terms and conditions for copying, distribution and 69 | modification follow. 70 | 71 | TERMS AND CONDITIONS 72 | 73 | 0. Definitions. 74 | 75 | "This License" refers to version 3 of the GNU General Public License. 76 | 77 | "Copyright" also means copyright-like laws that apply to other kinds of 78 | works, such as semiconductor masks. 79 | 80 | "The Program" refers to any copyrightable work licensed under this 81 | License. Each licensee is addressed as "you". "Licensees" and 82 | "recipients" may be individuals or organizations. 83 | 84 | To "modify" a work means to copy from or adapt all or part of the work 85 | in a fashion requiring copyright permission, other than the making of an 86 | exact copy. The resulting work is called a "modified version" of the 87 | earlier work or a work "based on" the earlier work. 88 | 89 | A "covered work" means either the unmodified Program or a work based 90 | on the Program. 91 | 92 | To "propagate" a work means to do anything with it that, without 93 | permission, would make you directly or secondarily liable for 94 | infringement under applicable copyright law, except executing it on a 95 | computer or modifying a private copy. Propagation includes copying, 96 | distribution (with or without modification), making available to the 97 | public, and in some countries other activities as well. 98 | 99 | To "convey" a work means any kind of propagation that enables other 100 | parties to make or receive copies. Mere interaction with a user through 101 | a computer network, with no transfer of a copy, is not conveying. 102 | 103 | An interactive user interface displays "Appropriate Legal Notices" 104 | to the extent that it includes a convenient and prominently visible 105 | feature that (1) displays an appropriate copyright notice, and (2) 106 | tells the user that there is no warranty for the work (except to the 107 | extent that warranties are provided), that licensees may convey the 108 | work under this License, and how to view a copy of this License. If 109 | the interface presents a list of user commands or options, such as a 110 | menu, a prominent item in the list meets this criterion. 111 | 112 | 1. Source Code. 113 | 114 | The "source code" for a work means the preferred form of the work 115 | for making modifications to it. "Object code" means any non-source 116 | form of a work. 117 | 118 | A "Standard Interface" means an interface that either is an official 119 | standard defined by a recognized standards body, or, in the case of 120 | interfaces specified for a particular programming language, one that 121 | is widely used among developers working in that language. 122 | 123 | The "System Libraries" of an executable work include anything, other 124 | than the work as a whole, that (a) is included in the normal form of 125 | packaging a Major Component, but which is not part of that Major 126 | Component, and (b) serves only to enable use of the work with that 127 | Major Component, or to implement a Standard Interface for which an 128 | implementation is available to the public in source code form. A 129 | "Major Component", in this context, means a major essential component 130 | (kernel, window system, and so on) of the specific operating system 131 | (if any) on which the executable work runs, or a compiler used to 132 | produce the work, or an object code interpreter used to run it. 133 | 134 | The "Corresponding Source" for a work in object code form means all 135 | the source code needed to generate, install, and (for an executable 136 | work) run the object code and to modify the work, including scripts to 137 | control those activities. However, it does not include the work's 138 | System Libraries, or general-purpose tools or generally available free 139 | programs which are used unmodified in performing those activities but 140 | which are not part of the work. For example, Corresponding Source 141 | includes interface definition files associated with source files for 142 | the work, and the source code for shared libraries and dynamically 143 | linked subprograms that the work is specifically designed to require, 144 | such as by intimate data communication or control flow between those 145 | subprograms and other parts of the work. 146 | 147 | The Corresponding Source need not include anything that users 148 | can regenerate automatically from other parts of the Corresponding 149 | Source. 150 | 151 | The Corresponding Source for a work in source code form is that 152 | same work. 153 | 154 | 2. Basic Permissions. 155 | 156 | All rights granted under this License are granted for the term of 157 | copyright on the Program, and are irrevocable provided the stated 158 | conditions are met. This License explicitly affirms your unlimited 159 | permission to run the unmodified Program. The output from running a 160 | covered work is covered by this License only if the output, given its 161 | content, constitutes a covered work. This License acknowledges your 162 | rights of fair use or other equivalent, as provided by copyright law. 163 | 164 | You may make, run and propagate covered works that you do not 165 | convey, without conditions so long as your license otherwise remains 166 | in force. You may convey covered works to others for the sole purpose 167 | of having them make modifications exclusively for you, or provide you 168 | with facilities for running those works, provided that you comply with 169 | the terms of this License in conveying all material for which you do 170 | not control copyright. Those thus making or running the covered works 171 | for you must do so exclusively on your behalf, under your direction 172 | and control, on terms that prohibit them from making any copies of 173 | your copyrighted material outside their relationship with you. 174 | 175 | Conveying under any other circumstances is permitted solely under 176 | the conditions stated below. Sublicensing is not allowed; section 10 177 | makes it unnecessary. 178 | 179 | 3. Protecting Users' Legal Rights From Anti-Circumvention Law. 180 | 181 | No covered work shall be deemed part of an effective technological 182 | measure under any applicable law fulfilling obligations under article 183 | 11 of the WIPO copyright treaty adopted on 20 December 1996, or 184 | similar laws prohibiting or restricting circumvention of such 185 | measures. 186 | 187 | When you convey a covered work, you waive any legal power to forbid 188 | circumvention of technological measures to the extent such circumvention 189 | is effected by exercising rights under this License with respect to 190 | the covered work, and you disclaim any intention to limit operation or 191 | modification of the work as a means of enforcing, against the work's 192 | users, your or third parties' legal rights to forbid circumvention of 193 | technological measures. 194 | 195 | 4. Conveying Verbatim Copies. 196 | 197 | You may convey verbatim copies of the Program's source code as you 198 | receive it, in any medium, provided that you conspicuously and 199 | appropriately publish on each copy an appropriate copyright notice; 200 | keep intact all notices stating that this License and any 201 | non-permissive terms added in accord with section 7 apply to the code; 202 | keep intact all notices of the absence of any warranty; and give all 203 | recipients a copy of this License along with the Program. 204 | 205 | You may charge any price or no price for each copy that you convey, 206 | and you may offer support or warranty protection for a fee. 207 | 208 | 5. Conveying Modified Source Versions. 209 | 210 | You may convey a work based on the Program, or the modifications to 211 | produce it from the Program, in the form of source code under the 212 | terms of section 4, provided that you also meet all of these conditions: 213 | 214 | a) The work must carry prominent notices stating that you modified 215 | it, and giving a relevant date. 216 | 217 | b) The work must carry prominent notices stating that it is 218 | released under this License and any conditions added under section 219 | 7. This requirement modifies the requirement in section 4 to 220 | "keep intact all notices". 221 | 222 | c) You must license the entire work, as a whole, under this 223 | License to anyone who comes into possession of a copy. This 224 | License will therefore apply, along with any applicable section 7 225 | additional terms, to the whole of the work, and all its parts, 226 | regardless of how they are packaged. This License gives no 227 | permission to license the work in any other way, but it does not 228 | invalidate such permission if you have separately received it. 229 | 230 | d) If the work has interactive user interfaces, each must display 231 | Appropriate Legal Notices; however, if the Program has interactive 232 | interfaces that do not display Appropriate Legal Notices, your 233 | work need not make them do so. 234 | 235 | A compilation of a covered work with other separate and independent 236 | works, which are not by their nature extensions of the covered work, 237 | and which are not combined with it such as to form a larger program, 238 | in or on a volume of a storage or distribution medium, is called an 239 | "aggregate" if the compilation and its resulting copyright are not 240 | used to limit the access or legal rights of the compilation's users 241 | beyond what the individual works permit. Inclusion of a covered work 242 | in an aggregate does not cause this License to apply to the other 243 | parts of the aggregate. 244 | 245 | 6. Conveying Non-Source Forms. 246 | 247 | You may convey a covered work in object code form under the terms 248 | of sections 4 and 5, provided that you also convey the 249 | machine-readable Corresponding Source under the terms of this License, 250 | in one of these ways: 251 | 252 | a) Convey the object code in, or embodied in, a physical product 253 | (including a physical distribution medium), accompanied by the 254 | Corresponding Source fixed on a durable physical medium 255 | customarily used for software interchange. 256 | 257 | b) Convey the object code in, or embodied in, a physical product 258 | (including a physical distribution medium), accompanied by a 259 | written offer, valid for at least three years and valid for as 260 | long as you offer spare parts or customer support for that product 261 | model, to give anyone who possesses the object code either (1) a 262 | copy of the Corresponding Source for all the software in the 263 | product that is covered by this License, on a durable physical 264 | medium customarily used for software interchange, for a price no 265 | more than your reasonable cost of physically performing this 266 | conveying of source, or (2) access to copy the 267 | Corresponding Source from a network server at no charge. 268 | 269 | c) Convey individual copies of the object code with a copy of the 270 | written offer to provide the Corresponding Source. This 271 | alternative is allowed only occasionally and noncommercially, and 272 | only if you received the object code with such an offer, in accord 273 | with subsection 6b. 274 | 275 | d) Convey the object code by offering access from a designated 276 | place (gratis or for a charge), and offer equivalent access to the 277 | Corresponding Source in the same way through the same place at no 278 | further charge. You need not require recipients to copy the 279 | Corresponding Source along with the object code. If the place to 280 | copy the object code is a network server, the Corresponding Source 281 | may be on a different server (operated by you or a third party) 282 | that supports equivalent copying facilities, provided you maintain 283 | clear directions next to the object code saying where to find the 284 | Corresponding Source. Regardless of what server hosts the 285 | Corresponding Source, you remain obligated to ensure that it is 286 | available for as long as needed to satisfy these requirements. 287 | 288 | e) Convey the object code using peer-to-peer transmission, provided 289 | you inform other peers where the object code and Corresponding 290 | Source of the work are being offered to the general public at no 291 | charge under subsection 6d. 292 | 293 | A separable portion of the object code, whose source code is excluded 294 | from the Corresponding Source as a System Library, need not be 295 | included in conveying the object code work. 296 | 297 | A "User Product" is either (1) a "consumer product", which means any 298 | tangible personal property which is normally used for personal, family, 299 | or household purposes, or (2) anything designed or sold for incorporation 300 | into a dwelling. In determining whether a product is a consumer product, 301 | doubtful cases shall be resolved in favor of coverage. For a particular 302 | product received by a particular user, "normally used" refers to a 303 | typical or common use of that class of product, regardless of the status 304 | of the particular user or of the way in which the particular user 305 | actually uses, or expects or is expected to use, the product. A product 306 | is a consumer product regardless of whether the product has substantial 307 | commercial, industrial or non-consumer uses, unless such uses represent 308 | the only significant mode of use of the product. 309 | 310 | "Installation Information" for a User Product means any methods, 311 | procedures, authorization keys, or other information required to install 312 | and execute modified versions of a covered work in that User Product from 313 | a modified version of its Corresponding Source. The information must 314 | suffice to ensure that the continued functioning of the modified object 315 | code is in no case prevented or interfered with solely because 316 | modification has been made. 317 | 318 | If you convey an object code work under this section in, or with, or 319 | specifically for use in, a User Product, and the conveying occurs as 320 | part of a transaction in which the right of possession and use of the 321 | User Product is transferred to the recipient in perpetuity or for a 322 | fixed term (regardless of how the transaction is characterized), the 323 | Corresponding Source conveyed under this section must be accompanied 324 | by the Installation Information. But this requirement does not apply 325 | if neither you nor any third party retains the ability to install 326 | modified object code on the User Product (for example, the work has 327 | been installed in ROM). 328 | 329 | The requirement to provide Installation Information does not include a 330 | requirement to continue to provide support service, warranty, or updates 331 | for a work that has been modified or installed by the recipient, or for 332 | the User Product in which it has been modified or installed. Access to a 333 | network may be denied when the modification itself materially and 334 | adversely affects the operation of the network or violates the rules and 335 | protocols for communication across the network. 336 | 337 | Corresponding Source conveyed, and Installation Information provided, 338 | in accord with this section must be in a format that is publicly 339 | documented (and with an implementation available to the public in 340 | source code form), and must require no special password or key for 341 | unpacking, reading or copying. 342 | 343 | 7. Additional Terms. 344 | 345 | "Additional permissions" are terms that supplement the terms of this 346 | License by making exceptions from one or more of its conditions. 347 | Additional permissions that are applicable to the entire Program shall 348 | be treated as though they were included in this License, to the extent 349 | that they are valid under applicable law. If additional permissions 350 | apply only to part of the Program, that part may be used separately 351 | under those permissions, but the entire Program remains governed by 352 | this License without regard to the additional permissions. 353 | 354 | When you convey a copy of a covered work, you may at your option 355 | remove any additional permissions from that copy, or from any part of 356 | it. (Additional permissions may be written to require their own 357 | removal in certain cases when you modify the work.) You may place 358 | additional permissions on material, added by you to a covered work, 359 | for which you have or can give appropriate copyright permission. 360 | 361 | Notwithstanding any other provision of this License, for material you 362 | add to a covered work, you may (if authorized by the copyright holders of 363 | that material) supplement the terms of this License with terms: 364 | 365 | a) Disclaiming warranty or limiting liability differently from the 366 | terms of sections 15 and 16 of this License; or 367 | 368 | b) Requiring preservation of specified reasonable legal notices or 369 | author attributions in that material or in the Appropriate Legal 370 | Notices displayed by works containing it; or 371 | 372 | c) Prohibiting misrepresentation of the origin of that material, or 373 | requiring that modified versions of such material be marked in 374 | reasonable ways as different from the original version; or 375 | 376 | d) Limiting the use for publicity purposes of names of licensors or 377 | authors of the material; or 378 | 379 | e) Declining to grant rights under trademark law for use of some 380 | trade names, trademarks, or service marks; or 381 | 382 | f) Requiring indemnification of licensors and authors of that 383 | material by anyone who conveys the material (or modified versions of 384 | it) with contractual assumptions of liability to the recipient, for 385 | any liability that these contractual assumptions directly impose on 386 | those licensors and authors. 387 | 388 | All other non-permissive additional terms are considered "further 389 | restrictions" within the meaning of section 10. If the Program as you 390 | received it, or any part of it, contains a notice stating that it is 391 | governed by this License along with a term that is a further 392 | restriction, you may remove that term. If a license document contains 393 | a further restriction but permits relicensing or conveying under this 394 | License, you may add to a covered work material governed by the terms 395 | of that license document, provided that the further restriction does 396 | not survive such relicensing or conveying. 397 | 398 | If you add terms to a covered work in accord with this section, you 399 | must place, in the relevant source files, a statement of the 400 | additional terms that apply to those files, or a notice indicating 401 | where to find the applicable terms. 402 | 403 | Additional terms, permissive or non-permissive, may be stated in the 404 | form of a separately written license, or stated as exceptions; 405 | the above requirements apply either way. 406 | 407 | 8. Termination. 408 | 409 | You may not propagate or modify a covered work except as expressly 410 | provided under this License. Any attempt otherwise to propagate or 411 | modify it is void, and will automatically terminate your rights under 412 | this License (including any patent licenses granted under the third 413 | paragraph of section 11). 414 | 415 | However, if you cease all violation of this License, then your 416 | license from a particular copyright holder is reinstated (a) 417 | provisionally, unless and until the copyright holder explicitly and 418 | finally terminates your license, and (b) permanently, if the copyright 419 | holder fails to notify you of the violation by some reasonable means 420 | prior to 60 days after the cessation. 421 | 422 | Moreover, your license from a particular copyright holder is 423 | reinstated permanently if the copyright holder notifies you of the 424 | violation by some reasonable means, this is the first time you have 425 | received notice of violation of this License (for any work) from that 426 | copyright holder, and you cure the violation prior to 30 days after 427 | your receipt of the notice. 428 | 429 | Termination of your rights under this section does not terminate the 430 | licenses of parties who have received copies or rights from you under 431 | this License. If your rights have been terminated and not permanently 432 | reinstated, you do not qualify to receive new licenses for the same 433 | material under section 10. 434 | 435 | 9. Acceptance Not Required for Having Copies. 436 | 437 | You are not required to accept this License in order to receive or 438 | run a copy of the Program. Ancillary propagation of a covered work 439 | occurring solely as a consequence of using peer-to-peer transmission 440 | to receive a copy likewise does not require acceptance. However, 441 | nothing other than this License grants you permission to propagate or 442 | modify any covered work. These actions infringe copyright if you do 443 | not accept this License. Therefore, by modifying or propagating a 444 | covered work, you indicate your acceptance of this License to do so. 445 | 446 | 10. Automatic Licensing of Downstream Recipients. 447 | 448 | Each time you convey a covered work, the recipient automatically 449 | receives a license from the original licensors, to run, modify and 450 | propagate that work, subject to this License. You are not responsible 451 | for enforcing compliance by third parties with this License. 452 | 453 | An "entity transaction" is a transaction transferring control of an 454 | organization, or substantially all assets of one, or subdividing an 455 | organization, or merging organizations. If propagation of a covered 456 | work results from an entity transaction, each party to that 457 | transaction who receives a copy of the work also receives whatever 458 | licenses to the work the party's predecessor in interest had or could 459 | give under the previous paragraph, plus a right to possession of the 460 | Corresponding Source of the work from the predecessor in interest, if 461 | the predecessor has it or can get it with reasonable efforts. 462 | 463 | You may not impose any further restrictions on the exercise of the 464 | rights granted or affirmed under this License. For example, you may 465 | not impose a license fee, royalty, or other charge for exercise of 466 | rights granted under this License, and you may not initiate litigation 467 | (including a cross-claim or counterclaim in a lawsuit) alleging that 468 | any patent claim is infringed by making, using, selling, offering for 469 | sale, or importing the Program or any portion of it. 470 | 471 | 11. Patents. 472 | 473 | A "contributor" is a copyright holder who authorizes use under this 474 | License of the Program or a work on which the Program is based. The 475 | work thus licensed is called the contributor's "contributor version". 476 | 477 | A contributor's "essential patent claims" are all patent claims 478 | owned or controlled by the contributor, whether already acquired or 479 | hereafter acquired, that would be infringed by some manner, permitted 480 | by this License, of making, using, or selling its contributor version, 481 | but do not include claims that would be infringed only as a 482 | consequence of further modification of the contributor version. For 483 | purposes of this definition, "control" includes the right to grant 484 | patent sublicenses in a manner consistent with the requirements of 485 | this License. 486 | 487 | Each contributor grants you a non-exclusive, worldwide, royalty-free 488 | patent license under the contributor's essential patent claims, to 489 | make, use, sell, offer for sale, import and otherwise run, modify and 490 | propagate the contents of its contributor version. 491 | 492 | In the following three paragraphs, a "patent license" is any express 493 | agreement or commitment, however denominated, not to enforce a patent 494 | (such as an express permission to practice a patent or covenant not to 495 | sue for patent infringement). To "grant" such a patent license to a 496 | party means to make such an agreement or commitment not to enforce a 497 | patent against the party. 498 | 499 | If you convey a covered work, knowingly relying on a patent license, 500 | and the Corresponding Source of the work is not available for anyone 501 | to copy, free of charge and under the terms of this License, through a 502 | publicly available network server or other readily accessible means, 503 | then you must either (1) cause the Corresponding Source to be so 504 | available, or (2) arrange to deprive yourself of the benefit of the 505 | patent license for this particular work, or (3) arrange, in a manner 506 | consistent with the requirements of this License, to extend the patent 507 | license to downstream recipients. "Knowingly relying" means you have 508 | actual knowledge that, but for the patent license, your conveying the 509 | covered work in a country, or your recipient's use of the covered work 510 | in a country, would infringe one or more identifiable patents in that 511 | country that you have reason to believe are valid. 512 | 513 | If, pursuant to or in connection with a single transaction or 514 | arrangement, you convey, or propagate by procuring conveyance of, a 515 | covered work, and grant a patent license to some of the parties 516 | receiving the covered work authorizing them to use, propagate, modify 517 | or convey a specific copy of the covered work, then the patent license 518 | you grant is automatically extended to all recipients of the covered 519 | work and works based on it. 520 | 521 | A patent license is "discriminatory" if it does not include within 522 | the scope of its coverage, prohibits the exercise of, or is 523 | conditioned on the non-exercise of one or more of the rights that are 524 | specifically granted under this License. You may not convey a covered 525 | work if you are a party to an arrangement with a third party that is 526 | in the business of distributing software, under which you make payment 527 | to the third party based on the extent of your activity of conveying 528 | the work, and under which the third party grants, to any of the 529 | parties who would receive the covered work from you, a discriminatory 530 | patent license (a) in connection with copies of the covered work 531 | conveyed by you (or copies made from those copies), or (b) primarily 532 | for and in connection with specific products or compilations that 533 | contain the covered work, unless you entered into that arrangement, 534 | or that patent license was granted, prior to 28 March 2007. 535 | 536 | Nothing in this License shall be construed as excluding or limiting 537 | any implied license or other defenses to infringement that may 538 | otherwise be available to you under applicable patent law. 539 | 540 | 12. No Surrender of Others' Freedom. 541 | 542 | If conditions are imposed on you (whether by court order, agreement or 543 | otherwise) that contradict the conditions of this License, they do not 544 | excuse you from the conditions of this License. If you cannot convey a 545 | covered work so as to satisfy simultaneously your obligations under this 546 | License and any other pertinent obligations, then as a consequence you may 547 | not convey it at all. For example, if you agree to terms that obligate you 548 | to collect a royalty for further conveying from those to whom you convey 549 | the Program, the only way you could satisfy both those terms and this 550 | License would be to refrain entirely from conveying the Program. 551 | 552 | 13. Use with the GNU Affero General Public License. 553 | 554 | Notwithstanding any other provision of this License, you have 555 | permission to link or combine any covered work with a work licensed 556 | under version 3 of the GNU Affero General Public License into a single 557 | combined work, and to convey the resulting work. The terms of this 558 | License will continue to apply to the part which is the covered work, 559 | but the special requirements of the GNU Affero General Public License, 560 | section 13, concerning interaction through a network will apply to the 561 | combination as such. 562 | 563 | 14. Revised Versions of this License. 564 | 565 | The Free Software Foundation may publish revised and/or new versions of 566 | the GNU General Public License from time to time. Such new versions will 567 | be similar in spirit to the present version, but may differ in detail to 568 | address new problems or concerns. 569 | 570 | Each version is given a distinguishing version number. If the 571 | Program specifies that a certain numbered version of the GNU General 572 | Public License "or any later version" applies to it, you have the 573 | option of following the terms and conditions either of that numbered 574 | version or of any later version published by the Free Software 575 | Foundation. If the Program does not specify a version number of the 576 | GNU General Public License, you may choose any version ever published 577 | by the Free Software Foundation. 578 | 579 | If the Program specifies that a proxy can decide which future 580 | versions of the GNU General Public License can be used, that proxy's 581 | public statement of acceptance of a version permanently authorizes you 582 | to choose that version for the Program. 583 | 584 | Later license versions may give you additional or different 585 | permissions. However, no additional obligations are imposed on any 586 | author or copyright holder as a result of your choosing to follow a 587 | later version. 588 | 589 | 15. Disclaimer of Warranty. 590 | 591 | THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY 592 | APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT 593 | HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY 594 | OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, 595 | THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 596 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM 597 | IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF 598 | ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 599 | 600 | 16. Limitation of Liability. 601 | 602 | IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING 603 | WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS 604 | THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY 605 | GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE 606 | USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF 607 | DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD 608 | PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), 609 | EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF 610 | SUCH DAMAGES. 611 | 612 | 17. Interpretation of Sections 15 and 16. 613 | 614 | If the disclaimer of warranty and limitation of liability provided 615 | above cannot be given local legal effect according to their terms, 616 | reviewing courts shall apply local law that most closely approximates 617 | an absolute waiver of all civil liability in connection with the 618 | Program, unless a warranty or assumption of liability accompanies a 619 | copy of the Program in return for a fee. 620 | 621 | END OF TERMS AND CONDITIONS 622 | 623 | How to Apply These Terms to Your New Programs 624 | 625 | If you develop a new program, and you want it to be of the greatest 626 | possible use to the public, the best way to achieve this is to make it 627 | free software which everyone can redistribute and change under these terms. 628 | 629 | To do so, attach the following notices to the program. It is safest 630 | to attach them to the start of each source file to most effectively 631 | state the exclusion of warranty; and each file should have at least 632 | the "copyright" line and a pointer to where the full notice is found. 633 | 634 | 635 | Copyright (C) 636 | 637 | This program is free software: you can redistribute it and/or modify 638 | it under the terms of the GNU General Public License as published by 639 | the Free Software Foundation, either version 3 of the License, or 640 | (at your option) any later version. 641 | 642 | This program is distributed in the hope that it will be useful, 643 | but WITHOUT ANY WARRANTY; without even the implied warranty of 644 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 645 | GNU General Public License for more details. 646 | 647 | You should have received a copy of the GNU General Public License 648 | along with this program. If not, see . 649 | 650 | Also add information on how to contact you by electronic and paper mail. 651 | 652 | If the program does terminal interaction, make it output a short 653 | notice like this when it starts in an interactive mode: 654 | 655 | Copyright (C) 656 | This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. 657 | This is free software, and you are welcome to redistribute it 658 | under certain conditions; type `show c' for details. 659 | 660 | The hypothetical commands `show w' and `show c' should show the appropriate 661 | parts of the General Public License. Of course, your program's commands 662 | might be different; for a GUI interface, you would use an "about box". 663 | 664 | You should also get your employer (if you work as a programmer) or school, 665 | if any, to sign a "copyright disclaimer" for the program, if necessary. 666 | For more information on this, and how to apply and follow the GNU GPL, see 667 | . 668 | 669 | The GNU General Public License does not permit incorporating your program 670 | into proprietary programs. If your program is a subroutine library, you 671 | may consider it more useful to permit linking proprietary applications with 672 | the library. If this is what you want to do, use the GNU Lesser General 673 | Public License instead of this License. But first, please read 674 | . 675 | -------------------------------------------------------------------------------- /Makefile: -------------------------------------------------------------------------------- 1 | .PHONY: stop, start, remove, open, build 2 | 3 | SERVICE_LIST = router yolo 4 | GPU_OPTIONS=--gpus all 5 | 6 | validate_service: 7 | ifeq ($(filter $(SERVICE),$(SERVICE_LIST)),) 8 | @echo Invalid SERVICE: [$(SERVICE)], valid values are [$(SERVICE_LIST)] 9 | $(error Invalid SERVICE, valid values are [$(SERVICE_LIST)]) 10 | endif 11 | 12 | stop: validate_service 13 | @echo "=> Stopping typefly-$(SERVICE)..." 14 | @-docker stop -t 0 typefly-$(SERVICE) > /dev/null 2>&1 15 | @-docker rm -f typefly-$(SERVICE) > /dev/null 2>&1 16 | 17 | start: validate_service 18 | @make stop SERVICE=$(SERVICE) 19 | @echo "=> Starting typefly-$(SERVICE)..." 20 | docker run -td --privileged --net=host $(GPU_OPTIONS) --ipc=host \ 21 | --env-file ./docker/env.list \ 22 | --name="typefly-$(SERVICE)" typefly-$(SERVICE):0.1 23 | 24 | remove: validate_service 25 | @echo "=> Removing typefly-$(SERVICE)..." 26 | @-docker image rm -f typefly-$(SERVICE):0.1 > /dev/null 2>&1 27 | @-docker rm -f typefly-$(SERVICE) > /dev/null 2>&1 28 | 29 | open: validate_service 30 | @echo "=> Opening bash in typefly-$(SERVICE)..." 31 | @docker exec -it typefly-$(SERVICE) bash 32 | 33 | build: validate_service 34 | @echo "=> Building typefly-$(SERVICE)..." 35 | @make stop SERVICE=$(SERVICE) 36 | @make remove SERVICE=$(SERVICE) 37 | @echo -n "=>" 38 | docker build -t typefly-$(SERVICE):0.1 -f ./docker/$(SERVICE)/Dockerfile . 39 | @echo -n "=>" 40 | @make start SERVICE=$(SERVICE) 41 | 42 | typefly: 43 | bash ./serving/webui/install_requirements.sh 44 | cd ./proto && bash generate.sh 45 | python3 ./serving/webui/typefly.py --use_virtual_robot -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # TypeFly 2 | TypeFly aims to generate robot task plan using large language model (LLM) and our custom programming language `MiniSpec`. Link to our [full Paper](https://drive.google.com/file/d/1COrozqEIk6v8DLxI3vCgoSUEWpnsc2mu/view) and [webpage](https://typefly.github.io/). 3 | 4 | Also, check out the demo video here: [Demo 1: Find edible or drinkable items](http://www.youtube.com/watch?v=HEJYaTLWKfY), [Demo 2: Find a specific chair](http://www.youtube.com/watch?v=QwnBniFaINE). 5 | 6 | ## Hardware Requirement 7 | TypeFly works with DJI Tello drone by default. Since Tello drone requires your device to connect to its wifi and TypeFly requires Internet connection, you need to have both wifi adapter and ethernet adapter to run TypeFly. 8 | To support other drones, you need to implement the `RobotWrapper` interface in `controller/abs/drone_wrapper.py`. 9 | 10 | ## OPENAI API KEY Requirement 11 | TypeFly use GPT-4 API as the remote LLM planner, please make sure you have set the `OPENAI_API_KEY` environment variable. 12 | 13 | ## Vision Encoder 14 | TypeFly uses YOLOv8 to generate the scene description. We provide the implementation of gRPC YOLO service and a optional http router to serve as a scheduler when working with multiple drones. We recommand using [docker](https://docs.docker.com/engine/install/ubuntu/) to run the YOLO and router. To deploy the YOLO servive with docker, please install the [Nvidia Container Toolkit](https://docs.nvidia.com/datacenter/cloud-native/container-toolkit/latest/install-guide.html), then run the following command: 15 | ```bash 16 | make SERVICE=yolo build 17 | ``` 18 | Optional: To deploy the router, please run the following command: 19 | ```bash 20 | make SERVICE=router build 21 | ``` 22 | 23 | ## TypeFly Web UI 24 | To play with the TypeFly web UI, please run the following command: 25 | ```bash 26 | make typefly 27 | ``` 28 | This will start the web UI at `http://localhost:50001` with your default camera (please make sure your device has a camera) and a virtual drone wrapper. You should be able to see the image capture window displayed with YOLO detection results. You can test the planning ability of TypeFly by typing in the chat box. 29 | 30 | To work with a real drone, please disable the `--use_virtual_robot` flag in `Makefile`. 31 | 32 | Here we assume your YOLO and router are deployed on the same machine running the TypeFly webui, if not, please define the environment variables `VISION_SERVICE_IP`, which is the IP address where you deploy your YOLO (or router) service, before running the webui. 33 | 34 | ## Task Execution 35 | Here are some examples of task descriptions, the `[Q]` prefix indicates TypeFly will output an answer to the question: 36 | - `Can you find something edible?` 37 | - `Can you see a person behind you?` 38 | - `[Q] Tell me how many people you can see?` 39 | -------------------------------------------------------------------------------- /controller/__init__.py: -------------------------------------------------------------------------------- 1 | from .yolo_client import YoloClient 2 | from .llm_controller import LLMController 3 | from .skillset import SkillSet, SkillItem, SkillArg -------------------------------------------------------------------------------- /controller/abs/robot_wrapper.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from enum import Enum 3 | 4 | class RobotType(Enum): 5 | VIRTUAL = 0 6 | TELLO = 1 7 | GEAR = 2 8 | 9 | class RobotWrapper(ABC): 10 | movement_x_accumulator = 0 11 | movement_y_accumulator = 0 12 | rotation_accumulator = 0 13 | @abstractmethod 14 | def connect(self): 15 | pass 16 | 17 | @abstractmethod 18 | def keep_active(self): 19 | pass 20 | 21 | @abstractmethod 22 | def takeoff(self) -> bool: 23 | pass 24 | 25 | @abstractmethod 26 | def land(self): 27 | pass 28 | 29 | @abstractmethod 30 | def start_stream(self): 31 | pass 32 | 33 | @abstractmethod 34 | def stop_stream(self): 35 | pass 36 | 37 | @abstractmethod 38 | def get_frame_reader(self): 39 | pass 40 | 41 | @abstractmethod 42 | def move_forward(self, distance: int) -> bool: 43 | pass 44 | 45 | @abstractmethod 46 | def move_backward(self, distance: int) -> bool: 47 | pass 48 | 49 | @abstractmethod 50 | def move_left(self, distance: int) -> bool: 51 | pass 52 | 53 | @abstractmethod 54 | def move_right(self, distance: int) -> bool: 55 | pass 56 | 57 | @abstractmethod 58 | def move_up(self, distance: int) -> bool: 59 | pass 60 | 61 | @abstractmethod 62 | def move_down(self, distance: int) -> bool: 63 | pass 64 | 65 | @abstractmethod 66 | def turn_ccw(self, degree: int) -> bool: 67 | pass 68 | 69 | @abstractmethod 70 | def turn_cw(self, degree: int) -> bool: 71 | pass 72 | -------------------------------------------------------------------------------- /controller/abs/skill_item.py: -------------------------------------------------------------------------------- 1 | from abc import ABC, abstractmethod 2 | from typing import List, Union, Tuple 3 | 4 | class SkillArg: 5 | def __init__(self, arg_name: str, arg_type: type): 6 | self.arg_name = arg_name 7 | self.arg_type = arg_type 8 | 9 | def __repr__(self): 10 | return f"{self.arg_name}:{self.arg_type.__name__}" 11 | 12 | class SkillItem(ABC): 13 | @abstractmethod 14 | def get_name(self) -> str: 15 | pass 16 | 17 | @abstractmethod 18 | def get_skill_description(self) -> str: 19 | pass 20 | 21 | @abstractmethod 22 | def get_argument(self) -> List[SkillArg]: 23 | pass 24 | 25 | @abstractmethod 26 | def __repr__(self) -> str: 27 | pass 28 | 29 | @abstractmethod 30 | def execute(self, arg_list: List[Union[int, float, str]]) -> Tuple[Union[int, float, bool, str], bool]: 31 | pass 32 | 33 | abbr_dict = {} 34 | def generate_abbreviation(self, word): 35 | split = word.split('_') 36 | abbr = ''.join([part[0] for part in split])[0:2] 37 | 38 | if abbr not in self.abbr_dict: 39 | self.abbr_dict[abbr] = word 40 | return abbr 41 | 42 | split = ''.join([part for part in split])[1:] 43 | 44 | count = 0 45 | while abbr in self.abbr_dict: 46 | abbr = abbr[0] + split[count] 47 | count += 1 48 | 49 | self.abbr_dict[abbr] = word 50 | return abbr 51 | 52 | def parse_args(self, args_str_list: List[Union[int, float, str]], allow_positional_args: bool = False): 53 | """Parses the string of arguments and converts them to the expected types.""" 54 | # Check the number of arguments 55 | if len(args_str_list) != len(self.args): 56 | raise ValueError(f"Expected {len(self.args)} arguments, but got {len(args_str_list)}.") 57 | 58 | parsed_args = [] 59 | for i, arg in enumerate(args_str_list): 60 | # if arg is not a string, skip parsing 61 | if not isinstance(arg, str): 62 | parsed_args.append(arg) 63 | continue 64 | # Allow positional arguments 65 | if arg.startswith('$') and allow_positional_args: 66 | parsed_args.append(arg) 67 | continue 68 | try: 69 | if self.args[i].arg_type == bool: 70 | parsed_args.append(arg.strip().lower() == 'true') 71 | else: 72 | parsed_args.append(self.args[i].arg_type(arg.strip())) 73 | except ValueError as e: 74 | raise ValueError(f"Error parsing argument {i + 1}. Expected type {self.args[i].arg_type.__name__}, but got value '{arg.strip()}'. Original error: {e}") 75 | return parsed_args -------------------------------------------------------------------------------- /controller/assets/Roboto-Medium.ttf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/typefly/TypeFly/6f40a91bd3e1dee971090c4d33b707c2a8dc5045/controller/assets/Roboto-Medium.ttf -------------------------------------------------------------------------------- /controller/assets/gear/model.pth: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/typefly/TypeFly/6f40a91bd3e1dee971090c4d33b707c2a8dc5045/controller/assets/gear/model.pth -------------------------------------------------------------------------------- /controller/assets/minispec_syntax.txt: -------------------------------------------------------------------------------- 1 | ::= { [';'] | ';' } 2 | ::= | | 3 | ::= | 4 | ::= '{' '}' 5 | ::= ['(' ')'] 6 | ::= '=' 7 | ::= '?' '{' '}' 8 | ::= { '&' | '|' } 9 | ::= '>' | '<' | '==' | '!=' 10 | ::= { } 11 | ::= { ',' } 12 | ::= '->' 13 | ::= | 14 | ::= | 15 | ::= '_' 16 | ::= | | | -------------------------------------------------------------------------------- /controller/assets/tello/guides.txt: -------------------------------------------------------------------------------- 1 | Handling Typos and Ambiguous Language: When encountering typos or vague language in user inputs, strive to clarify and correct these ambiguities. If a typo is evident, adjust the input to reflect the most likely intended meaning. For vague or unclear expressions, seek additional information from the user by returning a question to the user. 2 | Analytical Scene and Task Assessment: Evaluate the scene and task critically. If a specific condition or requirement is clearly deduced from the scene description, generate the corresponding action directly. 3 | Relevance to Current Scene: When considering tasks, assess their relevance to the current scene. If a task does not pertain to the existing scene, disregard any details from the current scene in formulating your response. 4 | Extraction of Key Information: Extract 'object_name' arguments or answers to questions primarily from the 'scene description'. If the necessary information is not present in the current scene, especially when the user asks the robot to move first or check somewhere else, use the probe 'p,' followed by the 'question', to determine the 'object_name' or answer subsequently. 5 | Handling Replan: When previous response and execution status are provided, it means part of the task has been executed. In this case, the system should replan the remaining task based on the current scene and the previous response. 6 | Under no circumstances should the system navigate the robot to hurt itself or others. If the task involves potential harm, the system should refuse to execute the task and provide a suitable explanation to the user. 7 | -------------------------------------------------------------------------------- /controller/assets/tello/high_level_skills copy.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "skill_name": "scan", 4 | "skill_description": "Rotate to find object $1 when it's *not* in current scene", 5 | "definition": "8{?iv($1)==True{->True}tc(45)}->False;" 6 | }, 7 | { 8 | "skill_name": "scan_abstract", 9 | "skill_description": "Rotate to find an abstract object by a description $1 when it's *not* in current scene", 10 | "definition": "8{_1=p($1);?_1!=False{->_1}tc(45)}->False;" 11 | }, 12 | { 13 | "skill_name": "orienting", 14 | "skill_description": "Rotate to align with object $1", 15 | "definition": "4{_1=ox($1);?_1>0.6{tc(15)};?_1<0.4{tu(15)};_2=ox($1);?_2<0.6&_2>0.4{->True}}->False;" 16 | }, 17 | { 18 | "skill_name": "approach", 19 | "skill_description": "Approach forward", 20 | "definition": "mf(100);" 21 | }, 22 | { 23 | "skill_name": "goto", 24 | "skill_description": "Go to object $1", 25 | "definition": "orienting($1);approach();" 26 | } 27 | ] -------------------------------------------------------------------------------- /controller/assets/tello/high_level_skills.json: -------------------------------------------------------------------------------- 1 | [ 2 | { 3 | "skill_name": "scan", 4 | "skill_description": "Rotate to find object $1 when it's *not* in current scene", 5 | "definition": "8{?iv($1)==True{->True}tc(45)}->False;" 6 | }, 7 | { 8 | "skill_name": "scan_abstract", 9 | "skill_description": "Rotate to find an abstract object by a description $1 when it's *not* in current scene", 10 | "definition": "8{_1=p($1);?_1!=False{->_1}tc(45)}->False;" 11 | } 12 | ] -------------------------------------------------------------------------------- /controller/assets/tello/plan_examples.txt: -------------------------------------------------------------------------------- 1 | Example 1: 2 | Scene: [] 3 | Task: [A] Find a bottle, tell me it's height and take a picture of it. 4 | Reason: no bottle instance in the scene, so we use scan to find bottle, then go and use object_height to get the height and log to output the height, finally use picture to take a picture of the bottle 5 | Response: ?s('bottle')==True{g('bottle');_2=oh('bottle');l(_2);tp}; 6 | 7 | Example 2: 8 | Scene: [apple x:0.28 y:0.52 width:0.13 height:0.2] 9 | Task: [A] Find an apple. 10 | Reason: there is an apple instance in the scene, we just go to it 11 | Response: g('apple'); 12 | 13 | Example 3: 14 | Scene: [apple x:0.28 y:0.15 width:0.2 height:0.19] 15 | Task: [Q] Is there an apple and an orange on your left? 16 | Reason: turn left 90 degrees, then use is_visible to check whether there is an apple on your left 17 | Response: tu(90);?iv('apple')==True&iv('orange'){l('Yes');->True}l('No');->False; 18 | 19 | Example 4: 20 | Scene: [chair x:0.58 y:0.5 width:0.43 height:0.7,laptop x:0.58 y:0.5 width:0.43 height:0.7] 21 | Task: [A] Turn around and go to the chair behind you. 22 | Reason: the chair is not the target because we want the one behind you. So we turn 180 degrees then go to the general object chair, since chair is a large object, we use 80cm as the distance. 23 | Response: tc(180);g('chair'); 24 | 25 | Example 5: 26 | Scene: [chair x:0.32 y:0.35 width:0.56 height:0.4] 27 | Task: [A] Find and go any edible object. 28 | Reason: edible object is abstract and there is no edible object in the scene, so we use scan_abstract to find the edible object 29 | Response: _1=sa('Any edible target here?');?_1!=False{g(_1)}; 30 | 31 | Example 6: 32 | Scene: [chair x:0.28 y:0.12 width:0.43 height:0.67,laptop x:0.78 y:0.45 width:0.23 height:0.25] 33 | Task: [A] Turn with 30 degrees step until you can see some animal. 34 | Reason: we use loop and probe to find animal 35 | Response: 12{_1=p('Any animal target here?');?_1!=False{l(_1);->True}tc(30)}->False; 36 | 37 | Example 7: 38 | Scene: [chair x:0.28 y:0.12 width:0.43 height:0.67,laptop x:0.28 y:0.12 width:0.43 height:0.67] 39 | Task: [A] If you can see a chair, go find a person, else go find an orange. 40 | Reason: From the scene, we can see a chair, so we use scan to find a person. Since person is a large object, we use 60cm as the distance 41 | Response: ?s('person')==True{g('person');->True}->False; 42 | 43 | Example 8: 44 | Scene: [chair x:0.48 y:0.22 width:0.23 height:0.17,chair x:0.18 y:0.12 width:0.33 height:0.27] 45 | Task: [A] Go to 46 | Reason: The task is too vague, so we use log to output the advice 47 | Response: l('Please give me more information.'); 48 | 49 | Example 9: 50 | Scene: [chair x:0.18 y:0.5 width:0.43 height:0.7, chair x:0.6 y:0.3 width:0.08 height:0.09, book x:0.62 y:0.26 width:0.23 height:0.17] 51 | Task: [A] Go to the chair with book on it. 52 | Reason: There are two chairs, the second one is closer to the book, so we go to the second chair 53 | Response: g('chair[0.6]'); 54 | 55 | Example 10: 56 | Scene: [apple x:0.74 y:0.3 width:0.09 height:0.1, apple x:0.3 y:0.5 width:0.15 height:0.12] 57 | Task: [A] Go to biggest apple 58 | Reason: from the scene, we tell directly that the right apple is the biggest apple 59 | Response: g('apple[0.3]'); 60 | 61 | Example 11: 62 | Scene: [apple x:0.74 y:0.3 width:0.09 height:0.1] 63 | Task: [A] Find a chair with a laptop on it. 64 | Reason: Using scan abstract to find the chair with a laptop on it 65 | Response: _1=sa('Any chair with a laptop on it?');?_1!=False{g(_1)}; 66 | 67 | Example 12: 68 | Scene: [sports ball x:0.74 y:0.3 width:0.09 height:0.1] 69 | Task: [A] Follow the ball for 17s. 70 | Reason: There is a ball in the scene, so we try to align with it and follow it for 17s 71 | Response: _1=ow('sports ball');500{?time()>17{->True}_2=ox('sports ball');?_2>0.55{tc(15)}?_2<0.45{tu(15)}_3=ow('sports ball');?_3>_1*1.2{mb(40)}?_3<_1*0.8{mf(40)}d(0.25)} 72 | 73 | Example 13: 74 | Scene: [chair x:0.18 y:0.5 width:0.43 height:0.7, chair x:0.6 y:0.3 width:0.08 height:0.09, book x:0.62 y:0.26 width:0.23 height:0.17] 75 | Task: [Q] Which chair is closer to the book? 76 | Reason: The second chair (on the right) is closer to the book 77 | Response: l('the right chair'); 78 | 79 | Example 14: 80 | Scene: [] 81 | Task: [A] Move in a 'V' shape. 82 | Reason: Go forward, then turn 135 degrees to the left, finally go forward again. 83 | Response: mf(100);tu(135);mf(100); -------------------------------------------------------------------------------- /controller/assets/tello/plan_examples_back_up.txt: -------------------------------------------------------------------------------- 1 | Example 1: 2 | Scene: [] 3 | Task: [A] Find a bottle, tell me it's height and take a picture of it. 4 | Reason: no bottle instance in the scene, so we use scan to find bottle, then go and use object_height to get the height and log to output the height, finally use picture to take a picture of the bottle 5 | Response: ?s('bottle')==True{g('bottle');_2=oh('bottle');l(_2);tp}; 6 | 7 | Example 2: 8 | Scene: [apple_5] 9 | Task: [A] Find an apple. 10 | Reason: there is an apple instance in the scene, we just go to the apple_5 11 | Response: g('apple_5'); 12 | 13 | Example 3: 14 | Scene: [apple_3] 15 | Task: [Q] Is there an apple and an orange on your left? 16 | Reason: turn left 90 degrees, then use is_visible to check whether there is an apple on your left 17 | Response: tu(90);?iv('apple')==True&iv('orange'){l('Yes');->True}l('No');->False; 18 | 19 | Example 4: 20 | Scene: [chair_13,laptop_2] 21 | Task: [A] Go to the chair behind you. 22 | Reason: the chair_13 is not the target because we want the one behind you. So we turn 180 degrees then go to the general object chair, since chair is a large object, we use 80cm as the distance. 23 | Response: tc(180);g('chair'); 24 | 25 | Example 5: 26 | Scene: [chair_3,laptop_1,bottle_5] 27 | Task: [A] Find and go any edible object. 28 | Reason: edible object is abstract and there is no edible object in the scene, so we use scan_abstract to find the edible object 29 | Response: _1=sa('Any edible target here?');?_1!=False{g(_1)}; 30 | 31 | Example 6: 32 | Scene: [chair_3,laptop_9] 33 | Task: [A] Turn around with 30 degrees step until you can see some animal. 34 | Reason: we use loop and probe to find animal 35 | Response: 12{_1=p('Any animal target here?');?_1!=False{l(_1);->True}tc(30)}->False; 36 | 37 | Example 7: 38 | Scene: [chair_3,laptop_9] 39 | Task: [A] If you can see a chair, go find a person, else go find an orange. 40 | Reason: From the scene, we can see a chair, so we use scan to find a person. Since person is a large object, we use 60cm as the distance 41 | Response: _1=s('person');?_1==True{g('person');->True}->False; 42 | 43 | Example 8: 44 | Scene: [chair_3,laptop_9] 45 | Task: [A] Go to 46 | Reason: The task is too vague, so we use log to output the advice 47 | Response: l('Please give me more information.'); 48 | 49 | Example 9: 50 | Scene: [chair_1 x:0.58 y:0.5 width:0.43 height:0.7, apple_1 x:0.6 y:0.3 width:0.08 height:0.09] 51 | Task: [A] Turn around and go to the apple 52 | Reason: after turning around, we will do replan. We found that the chair is blocking the apple, so we use moving_up to get over the chair and then go to the apple 53 | Response: mu(40);g('apple'); 54 | 55 | Example 10: 56 | Scene: [apple_1 x:0.34 y:0.3 width:0.09 height:0.1, apple_2 x:0.3 y:0.5 width:0.15 height:0.12] 57 | Task: [A] Go to biggest apple 58 | Reason: from the scene, we tell directly that apple_2 is the biggest apple 59 | Response: g('apple_2'); -------------------------------------------------------------------------------- /controller/assets/tello/prompt_plan.txt: -------------------------------------------------------------------------------- 1 | You are a robot pilot and you should follow the user's instructions to generate a MiniSpec plan to fulfill the task or give advice on user's input if it's not clear or not reasonable. 2 | 3 | Your response should carefully consider the 'system skills description', the 'scene description', the 'task description', and both the 'previous response' and the 'execution status' if they are provided. 4 | The 'system skills description' describes the system's capabilities which include low-level and high-level skills. Low-level skills, while fixed, offer direct function calls to control the robot and acquire vision information. High-level skills, built with our language 'MiniSpec', are more flexible and can be used to build more complex skills. Whenever possible, please prioritize the use of high-level skills, invoke skills using their designated abbreviations, and ensure that 'object_name' refers to a specific type of object. If a skill has no argument, you can call it without parentheses. 5 | 6 | Description of the two skill sets: 7 | - High-level skills: 8 | {system_skill_description_high} 9 | - Low-level skills: 10 | {system_skill_description_low} 11 | 12 | The 'scene description' is an object list of the current view, containing their names with ID, location, and size (location and size are floats between 0~1). This may not be useful if the task is about the environment outside the view. 13 | The 'task description' is a natural language sentence, describing the user's instructions. It may start with "[A]" or "[Q]". "[A]" sentences mean you should generate an execution plan for the robot. "[Q]" sentences mean you should use 'log' to show a literal answer at the end of the plan execution. Please carefully reason about the 'task description', you should interpret it and generate a detailed multi-step plan to achieve it as much as you can 14 | The 'execution history' is the actions have been taken from previous response. When they are provided, that means the robot is doing replanning, and the user wants to continue the task based on the task and history. You should reason about the 'execution history' and generate a new response accordingly. 15 | 16 | Here are some extra guides for you to better understand the task and generate a better response: 17 | {guides} 18 | 19 | Here is a list of example 'response' for different 'scene description' and 'task description', and their explanations: 20 | {plan_examples} 21 | 22 | Here is the 'scene description': 23 | {scene_description} 24 | 25 | Here is the 'task description': 26 | {task_description} 27 | 28 | Here is the 'execution history' (has value if replanning): 29 | {execution_history} 30 | 31 | Please generate the response only with a single sentence of MiniSpec program. 32 | 'response': -------------------------------------------------------------------------------- /controller/assets/tello/prompt_probe.txt: -------------------------------------------------------------------------------- 1 | """ 2 | You are given a scene description and a question. You should output the answer to the question based on the scene description. 3 | The scene description contains listed objects with their respective names, locations, and sizes. 4 | The question is a string that asks about the scene or the objects in the scene. 5 | For yes-or-no questions, output with 'True' or 'False' only. 6 | For object identification, output the object's name (if there are multiple same objects, output the target one with x value). If the object is not in the list, output with 'False'. 7 | For counting questions, output the exact number of target objects. 8 | For general questions, output a brief, single-sentence answer. 9 | 10 | Input Format: 11 | Scene Description:[List of Objects with Attributes] 12 | Question:[A String] 13 | 14 | Output Format: 15 | [A String] 16 | 17 | Here are some examples: 18 | Example 1: 19 | Scene Description:[person x:0.59 y:0.55 width:0.81 height:0.91, bottle x:0.85 y:0.54 width:0.21 height:0.93] 20 | Question:'Any drinkable target here?' 21 | Output:bottle 22 | 23 | Example 2: 24 | Scene Description:[] 25 | Question:'Any table in the room?' 26 | Output:False 27 | 28 | Example 3: 29 | Scene Description:[chair x:0.1 y:0.35 width:0.56 height:0.41, chair x:0.49 y:0.59 width:0.61 height:0.35] 30 | Question:'How many chairs you can see?' 31 | Output:2 32 | 33 | Example 4: 34 | Scene Description:[bottle x:0.1 y:0.35 width:0.56 height:0.41, chair x:0.49 y:0.59 width:0.61 height:0.35] 35 | Question:'Any edible target here?' 36 | Output:False 37 | 38 | Example 5: 39 | Scene Description:[chair x:0.18 y:0.5 width:0.43 height:0.7, chair x:0.6 y:0.3 width:0.08 height:0.09, book x:0.62 y:0.26 width:0.23 height:0.17] 40 | Question:'Any chair with a laptop on it?' 41 | Output:chair[0.6] 42 | """ 43 | Scene Description:{scene_description} 44 | Question:{question} 45 | Please give the content of results only, don't include 'Output:' in the results. -------------------------------------------------------------------------------- /controller/gear_wrapper.py: -------------------------------------------------------------------------------- 1 | import time, os 2 | from typing import Tuple 3 | from .abs.robot_wrapper import RobotWrapper 4 | from podtp import Podtp 5 | import torch 6 | import torch.nn as nn 7 | import numpy as np 8 | 9 | DEFAULT_NO_VALID_READING = 0 10 | SAFE_DISTANCE_THRESHOLD = 250 11 | SIDE_DISTANCE_THRESHOLD = 65 12 | JUMP_DISTANCE_THRESHOLD = 60 13 | 14 | CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) 15 | 16 | def clean_sensor_data(raw_data): 17 | cleaned_data = raw_data[:] # Create a copy of the raw data for cleaning 18 | 19 | for i in range(len(cleaned_data)): 20 | if cleaned_data[i] < 0: 21 | valid_previous = None 22 | valid_next = None 23 | 24 | # Find the previous valid value 25 | for j in range(i-1, -1, -1): 26 | if cleaned_data[j] >= 0: 27 | valid_previous = cleaned_data[j] 28 | break 29 | 30 | # Find the next valid value 31 | for k in range(i+1, len(cleaned_data)): 32 | if cleaned_data[k] >= 0: 33 | valid_next = cleaned_data[k] 34 | break 35 | 36 | # Decide what value to assign to the bad reading 37 | if valid_previous is not None and valid_next is not None: 38 | # Average if both previous and next valid values are found 39 | cleaned_data[i] = (valid_previous + valid_next) / 2 40 | elif valid_previous is not None: 41 | # Use the previous if only it is available 42 | cleaned_data[i] = valid_previous 43 | elif valid_next is not None: 44 | # Use the next if only it is available 45 | cleaned_data[i] = valid_next 46 | else: 47 | # If no valid readings are available, handle it with a default value or recheck 48 | cleaned_data[i] = DEFAULT_NO_VALID_READING 49 | 50 | return cleaned_data 51 | 52 | # Define the model 53 | class DirectionPredictor(nn.Module): 54 | def __init__(self): 55 | super(DirectionPredictor, self).__init__() 56 | self.flatten = nn.Flatten() 57 | self.linear_relu_stack = nn.Sequential( 58 | nn.Linear(24, 64), 59 | nn.ReLU(), 60 | nn.Linear(64, 64), 61 | nn.ReLU(), 62 | nn.Linear(64, 3) 63 | ) 64 | self.mean = 381.9494323730469 65 | self.std = 306.9201965332031 66 | 67 | def forward(self, x): 68 | x = self.flatten(x) 69 | logits = self.linear_relu_stack(x) 70 | return logits 71 | 72 | class GearWrapper(RobotWrapper): 73 | def __init__(self): 74 | self.stream_on = False 75 | config = { 76 | 'ip': '192.168.8.169', 77 | 'ip1': '192.168.8.195', 78 | 'port': 80, 79 | 'stream_port': 81 80 | } 81 | self.robot = Podtp(config) 82 | self.move_speed_x = 2.5 83 | self.move_speed_y = 2.8 84 | self.unlock_count = 0 85 | self.model = DirectionPredictor() 86 | self.model.load_state_dict(torch.load(os.path.join(CURRENT_DIR, 'assets/gear/model.pth'))) 87 | self.model.eval() 88 | 89 | def keep_active(self): 90 | self.unlock_count += 1 91 | if self.unlock_count > 100: 92 | self.robot.send_ctrl_lock(False) 93 | self.unlock_count = 0 94 | 95 | def connect(self): 96 | if not self.robot.connect(): 97 | raise ValueError("Could not connect to the robot") 98 | if not self.robot.send_ctrl_lock(False): 99 | raise ValueError("Could not unlock the robot control") 100 | 101 | def takeoff(self) -> bool: 102 | return True 103 | 104 | def land(self): 105 | pass 106 | 107 | def start_stream(self): 108 | self.robot.start_stream() 109 | self.stream_on = True 110 | 111 | def stop_stream(self): 112 | self.robot.stop_stream() 113 | self.stream_on = False 114 | 115 | def get_frame_reader(self): 116 | if not self.stream_on: 117 | return None 118 | return self.robot.sensor_data 119 | 120 | def move_forward(self, distance: int) -> Tuple[bool, bool]: 121 | print(f"-> Moving forward {distance} cm") 122 | self.robot.send_command_hover(0, 0, 0, 0) 123 | small_move = distance <= 15 124 | while distance > 0: 125 | if small_move: 126 | self.robot.send_command_hover(0, self.move_speed_x, 0, 0) 127 | else: 128 | array = self.robot.sensor_data.depth.data 129 | left_distance = clean_sensor_data(array[0, :]) 130 | front_distance = clean_sensor_data(array[2, :]) 131 | right_distance = clean_sensor_data(array[7, :]) 132 | if max(front_distance) < 50: 133 | self.move_backward(10) 134 | 135 | x = np.concatenate((left_distance, front_distance, right_distance)) 136 | x = torch.tensor(x, dtype=torch.float32) 137 | x = (x - self.model.mean) / self.model.std 138 | y = self.model(x.unsqueeze(0)).squeeze(0) 139 | command = torch.argmax(y).item() - 1 140 | 141 | left_margin = min(left_distance) 142 | right_margin = min(right_distance) 143 | if left_margin > SIDE_DISTANCE_THRESHOLD and right_margin > SIDE_DISTANCE_THRESHOLD: 144 | vy = 0 145 | elif left_margin > SIDE_DISTANCE_THRESHOLD: 146 | vy = -1.5 147 | elif right_margin > SIDE_DISTANCE_THRESHOLD: 148 | vy = 1.5 149 | else: 150 | if abs(left_margin - right_margin) > 80: 151 | if left_margin < right_margin: 152 | vy = 1.5 153 | else: 154 | vy = -1.5 155 | 156 | if command == 0: 157 | self.robot.send_command_hover(0, self.move_speed_x, vy, 0) 158 | elif command == 1: 159 | self.turn_ccw(30) 160 | elif command == -1: 161 | self.turn_cw(30) 162 | time.sleep(0.1) 163 | distance -= 2 164 | self.robot.send_command_hover(0, 0, 0, 0) 165 | return True, False 166 | 167 | def move_backward(self, distance: int) -> Tuple[bool, bool]: 168 | print(f"-> Moving backward {distance} cm") 169 | self.robot.send_command_hover(0, 0, 0, 0) 170 | while distance > 0: 171 | self.robot.send_command_hover(0, -self.move_speed_x, 0, 0) 172 | time.sleep(0.1) 173 | distance -= 2 174 | self.robot.send_command_hover(0, 0, 0, 0) 175 | return True, False 176 | 177 | def move_left(self, distance: int) -> Tuple[bool, bool]: 178 | print(f"-> Moving left {distance} cm") 179 | self.robot.send_command_hover(0, 0, 0, 0) 180 | while distance > 0: 181 | self.robot.send_command_hover(0, 0, -self.move_speed_y, 0) 182 | time.sleep(0.1) 183 | distance -= 2 184 | self.robot.send_command_hover(0, 0, 0, 0) 185 | return True, False 186 | 187 | def move_right(self, distance: int) -> Tuple[bool, bool]: 188 | print(f"-> Moving right {distance} cm") 189 | self.robot.send_command_hover(0, 0, 0, 0) 190 | while distance > 0: 191 | self.robot.send_command_hover(0, 0, self.move_speed_y, 0) 192 | time.sleep(0.1) 193 | distance -= 2 194 | self.robot.send_command_hover(0, 0, 0, 0) 195 | return True, False 196 | 197 | def move_up(self, distance: int) -> Tuple[bool, bool]: 198 | print(f"-> Moving up {distance} cm") 199 | return True, False 200 | 201 | def move_down(self, distance: int) -> Tuple[bool, bool]: 202 | print(f"-> Moving down {distance} cm") 203 | return True, False 204 | 205 | def turn_ccw(self, degree: int) -> Tuple[bool, bool]: 206 | print(f"-> Turning CCW {degree} degrees") 207 | self.robot.send_command_hover(0, 0, 0, 0) 208 | self.robot.send_command_position(0, 0, 0, degree) 209 | time.sleep(1 + degree / 50.0) 210 | self.robot.send_command_hover(0, 0, 0, 0) 211 | # if degree >= 90: 212 | # print("-> Turning CCW over 90 degrees") 213 | # return True, True 214 | return True, False 215 | 216 | def turn_cw(self, degree: int) -> Tuple[bool, bool]: 217 | print(f"-> Turning CW {degree} degrees") 218 | self.robot.send_command_hover(0, 0, 0, 0) 219 | self.robot.send_command_position(0, 0, 0, -degree) 220 | time.sleep(1 + degree / 50.0) 221 | self.robot.send_command_hover(0, 0, 0, 0) 222 | # if degree >= 90: 223 | # print("-> Turning CW over 90 degrees") 224 | # return True, True 225 | return True, False 226 | 227 | def move_in_circle(self, cw) -> Tuple[bool, bool]: 228 | if cw: 229 | vy = -8 230 | vr = -12 231 | else: 232 | vy = 8 233 | vr = 12 234 | for i in range(50): 235 | self.robot.send_command_hover(0, 0, vy, vr) 236 | time.sleep(0.1) 237 | self.robot.send_command_hover(0, 0, 0, 0) 238 | return True, False 239 | -------------------------------------------------------------------------------- /controller/llm_controller.py: -------------------------------------------------------------------------------- 1 | from PIL import Image 2 | import queue, time, os, json 3 | from typing import Optional, Tuple 4 | import asyncio 5 | import uuid 6 | from enum import Enum 7 | 8 | from .shared_frame import SharedFrame, Frame 9 | from .yolo_client import YoloClient 10 | from .yolo_grpc_client import YoloGRPCClient 11 | from .tello_wrapper import TelloWrapper 12 | from .virtual_robot_wrapper import VirtualRobotWrapper 13 | from .abs.robot_wrapper import RobotWrapper 14 | from .vision_skill_wrapper import VisionSkillWrapper 15 | from .llm_planner import LLMPlanner 16 | from .skillset import SkillSet, LowLevelSkillItem, HighLevelSkillItem, SkillArg 17 | from .utils import print_t, input_t 18 | from .minispec_interpreter import MiniSpecInterpreter, Statement 19 | from .abs.robot_wrapper import RobotType 20 | 21 | CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) 22 | 23 | class LLMController(): 24 | def __init__(self, robot_type, use_http=False, message_queue: Optional[queue.Queue]=None): 25 | self.shared_frame = SharedFrame() 26 | if use_http: 27 | self.yolo_client = YoloClient(shared_frame=self.shared_frame) 28 | else: 29 | self.yolo_client = YoloGRPCClient(shared_frame=self.shared_frame) 30 | self.vision = VisionSkillWrapper(self.shared_frame) 31 | self.latest_frame = None 32 | self.controller_active = True 33 | self.controller_wait_takeoff = True 34 | self.message_queue = message_queue 35 | if message_queue is None: 36 | self.cache_folder = os.path.join(CURRENT_DIR, 'cache') 37 | else: 38 | self.cache_folder = message_queue.get() 39 | 40 | if not os.path.exists(self.cache_folder): 41 | os.makedirs(self.cache_folder) 42 | 43 | match robot_type: 44 | case RobotType.TELLO: 45 | print_t("[C] Start Tello drone...") 46 | self.drone: RobotWrapper = TelloWrapper() 47 | case RobotType.GEAR: 48 | print_t("[C] Start Gear robot car...") 49 | from .gear_wrapper import GearWrapper 50 | self.drone: RobotWrapper = GearWrapper() 51 | case _: 52 | print_t("[C] Start virtual drone...") 53 | self.drone: RobotWrapper = VirtualRobotWrapper() 54 | 55 | self.planner = LLMPlanner(robot_type) 56 | 57 | # load low-level skills 58 | self.low_level_skillset = SkillSet(level="low") 59 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_forward", self.drone.move_forward, "Move forward by a distance", args=[SkillArg("distance", int)])) 60 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_backward", self.drone.move_backward, "Move backward by a distance", args=[SkillArg("distance", int)])) 61 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_left", self.drone.move_left, "Move left by a distance", args=[SkillArg("distance", int)])) 62 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_right", self.drone.move_right, "Move right by a distance", args=[SkillArg("distance", int)])) 63 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_up", self.drone.move_up, "Move up by a distance", args=[SkillArg("distance", int)])) 64 | self.low_level_skillset.add_skill(LowLevelSkillItem("move_down", self.drone.move_down, "Move down by a distance", args=[SkillArg("distance", int)])) 65 | self.low_level_skillset.add_skill(LowLevelSkillItem("turn_cw", self.drone.turn_cw, "Rotate clockwise/right by certain degrees", args=[SkillArg("degrees", int)])) 66 | self.low_level_skillset.add_skill(LowLevelSkillItem("turn_ccw", self.drone.turn_ccw, "Rotate counterclockwise/left by certain degrees", args=[SkillArg("degrees", int)])) 67 | self.low_level_skillset.add_skill(LowLevelSkillItem("delay", self.skill_delay, "Wait for specified seconds", args=[SkillArg("seconds", float)])) 68 | self.low_level_skillset.add_skill(LowLevelSkillItem("is_visible", self.vision.is_visible, "Check the visibility of target object", args=[SkillArg("object_name", str)])) 69 | self.low_level_skillset.add_skill(LowLevelSkillItem("object_x", self.vision.object_x, "Get object's X-coordinate in (0,1)", args=[SkillArg("object_name", str)])) 70 | self.low_level_skillset.add_skill(LowLevelSkillItem("object_y", self.vision.object_y, "Get object's Y-coordinate in (0,1)", args=[SkillArg("object_name", str)])) 71 | self.low_level_skillset.add_skill(LowLevelSkillItem("object_width", self.vision.object_width, "Get object's width in (0,1)", args=[SkillArg("object_name", str)])) 72 | self.low_level_skillset.add_skill(LowLevelSkillItem("object_height", self.vision.object_height, "Get object's height in (0,1)", args=[SkillArg("object_name", str)])) 73 | self.low_level_skillset.add_skill(LowLevelSkillItem("object_dis", self.vision.object_distance, "Get object's distance in cm", args=[SkillArg("object_name", str)])) 74 | self.low_level_skillset.add_skill(LowLevelSkillItem("probe", self.planner.probe, "Probe the LLM for reasoning", args=[SkillArg("question", str)])) 75 | self.low_level_skillset.add_skill(LowLevelSkillItem("log", self.skill_log, "Output text to console", args=[SkillArg("text", str)])) 76 | self.low_level_skillset.add_skill(LowLevelSkillItem("take_picture", self.skill_take_picture, "Take a picture")) 77 | self.low_level_skillset.add_skill(LowLevelSkillItem("re_plan", self.skill_re_plan, "Replanning")) 78 | 79 | self.low_level_skillset.add_skill(LowLevelSkillItem("goto", self.skill_goto, "goto the object", args=[SkillArg("object_name[*x-value]", str)])) 80 | self.low_level_skillset.add_skill(LowLevelSkillItem("time", self.skill_time, "Get current execution time", args=[])) 81 | # load high-level skills 82 | self.high_level_skillset = SkillSet(level="high", lower_level_skillset=self.low_level_skillset) 83 | 84 | type_folder_name = 'tello' 85 | if robot_type == RobotType.GEAR: 86 | type_folder_name = 'gear' 87 | with open(os.path.join(CURRENT_DIR, f"assets/{type_folder_name}/high_level_skills.json"), "r") as f: 88 | json_data = json.load(f) 89 | for skill in json_data: 90 | self.high_level_skillset.add_skill(HighLevelSkillItem.load_from_dict(skill)) 91 | 92 | Statement.low_level_skillset = self.low_level_skillset 93 | Statement.high_level_skillset = self.high_level_skillset 94 | self.planner.init(high_level_skillset=self.high_level_skillset, low_level_skillset=self.low_level_skillset, vision_skill=self.vision) 95 | 96 | self.current_plan = None 97 | self.execution_history = None 98 | self.execution_time = time.time() 99 | 100 | def skill_time(self) -> Tuple[float, bool]: 101 | return time.time() - self.execution_time, False 102 | 103 | def skill_goto(self, object_name: str) -> Tuple[None, bool]: 104 | print(f'Goto {object_name}') 105 | if '[' in object_name: 106 | x = float(object_name.split('[')[1].split(']')[0]) 107 | else: 108 | x = self.vision.object_x(object_name)[0] 109 | 110 | print(f'>> GOTO x {x} {type(x)}') 111 | 112 | if x > 0.55: 113 | self.drone.turn_cw(int((x - 0.5) * 70)) 114 | elif x < 0.45: 115 | self.drone.turn_ccw(int((0.5 - x) * 70)) 116 | 117 | self.drone.move_forward(110) 118 | return None, False 119 | 120 | def skill_take_picture(self) -> Tuple[None, bool]: 121 | img_path = os.path.join(self.cache_folder, f"{uuid.uuid4()}.jpg") 122 | Image.fromarray(self.latest_frame).save(img_path) 123 | print_t(f"[C] Picture saved to {img_path}") 124 | self.append_message((img_path,)) 125 | return None, False 126 | 127 | def skill_log(self, text: str) -> Tuple[None, bool]: 128 | self.append_message(f"[LOG] {text}") 129 | print_t(f"[LOG] {text}") 130 | return None, False 131 | 132 | def skill_re_plan(self) -> Tuple[None, bool]: 133 | return None, True 134 | 135 | def skill_delay(self, s: float) -> Tuple[None, bool]: 136 | time.sleep(s) 137 | return None, False 138 | 139 | def append_message(self, message: str): 140 | if self.message_queue is not None: 141 | self.message_queue.put(message) 142 | 143 | def stop_controller(self): 144 | self.controller_active = False 145 | 146 | def get_latest_frame(self, plot=False): 147 | image = self.shared_frame.get_image() 148 | if plot and image: 149 | self.vision.update() 150 | YoloClient.plot_results_oi(image, self.vision.object_list) 151 | return image 152 | 153 | def execute_minispec(self, minispec: str): 154 | interpreter = MiniSpecInterpreter(self.message_queue) 155 | interpreter.execute(minispec) 156 | self.execution_history = interpreter.execution_history 157 | ret_val = interpreter.ret_queue.get() 158 | return ret_val 159 | 160 | def execute_task_description(self, task_description: str): 161 | if self.controller_wait_takeoff: 162 | self.append_message("[Warning] Controller is waiting for takeoff...") 163 | return 164 | self.append_message('[TASK]: ' + task_description) 165 | ret_val = None 166 | while True: 167 | self.current_plan = self.planner.plan(task_description, execution_history=self.execution_history) 168 | self.append_message(f'[Plan]: \\\\') 169 | try: 170 | self.execution_time = time.time() 171 | ret_val = self.execute_minispec(self.current_plan) 172 | except Exception as e: 173 | print_t(f"[C] Error: {e}") 174 | 175 | # disable replan for debugging 176 | break 177 | if ret_val.replan: 178 | print_t(f"[C] > Replanning <: {ret_val.value}") 179 | continue 180 | else: 181 | break 182 | self.append_message(f'\n[Task ended]') 183 | self.append_message('end') 184 | self.current_plan = None 185 | self.execution_history = None 186 | 187 | def start_robot(self): 188 | print_t("[C] Connecting to robot...") 189 | self.drone.connect() 190 | print_t("[C] Starting robot...") 191 | self.drone.takeoff() 192 | self.drone.move_up(25) 193 | print_t("[C] Starting stream...") 194 | self.drone.start_stream() 195 | self.controller_wait_takeoff = False 196 | 197 | def stop_robot(self): 198 | print_t("[C] Drone is landing...") 199 | self.drone.land() 200 | self.drone.stop_stream() 201 | self.controller_wait_takeoff = True 202 | 203 | def capture_loop(self, asyncio_loop): 204 | print_t("[C] Start capture loop...") 205 | frame_reader = self.drone.get_frame_reader() 206 | while self.controller_active: 207 | self.drone.keep_active() 208 | self.latest_frame = frame_reader.frame 209 | frame = Frame(frame_reader.frame, 210 | frame_reader.depth if hasattr(frame_reader, 'depth') else None) 211 | 212 | if self.yolo_client.is_local_service(): 213 | self.yolo_client.detect_local(frame) 214 | else: 215 | # asynchronously send image to yolo server 216 | asyncio_loop.call_soon_threadsafe(asyncio.create_task, self.yolo_client.detect(frame)) 217 | time.sleep(0.10) 218 | # Cancel all running tasks (if any) 219 | for task in asyncio.all_tasks(asyncio_loop): 220 | task.cancel() 221 | self.drone.stop_stream() 222 | self.drone.land() 223 | asyncio_loop.stop() 224 | print_t("[C] Capture loop stopped") -------------------------------------------------------------------------------- /controller/llm_planner.py: -------------------------------------------------------------------------------- 1 | import os, ast 2 | from typing import Optional 3 | 4 | from .skillset import SkillSet 5 | from .llm_wrapper import LLMWrapper, GPT3, GPT4 6 | from .vision_skill_wrapper import VisionSkillWrapper 7 | from .utils import print_t 8 | from .minispec_interpreter import MiniSpecValueType, evaluate_value 9 | from .abs.robot_wrapper import RobotType 10 | 11 | CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) 12 | 13 | class LLMPlanner(): 14 | def __init__(self, robot_type: RobotType): 15 | self.llm = LLMWrapper() 16 | self.model_name = GPT4 17 | 18 | type_folder_name = 'tello' 19 | if robot_type == RobotType.GEAR: 20 | type_folder_name = 'gear' 21 | 22 | # read prompt from txt 23 | with open(os.path.join(CURRENT_DIR, f"./assets/{type_folder_name}/prompt_plan.txt"), "r") as f: 24 | self.prompt_plan = f.read() 25 | 26 | with open(os.path.join(CURRENT_DIR, f"./assets/{type_folder_name}/prompt_probe.txt"), "r") as f: 27 | self.prompt_probe = f.read() 28 | 29 | with open(os.path.join(CURRENT_DIR, f"./assets/{type_folder_name}/guides.txt"), "r") as f: 30 | self.guides = f.read() 31 | 32 | with open(os.path.join(CURRENT_DIR, f"./assets/{type_folder_name}/plan_examples.txt"), "r") as f: 33 | self.plan_examples = f.read() 34 | 35 | def set_model(self, model_name): 36 | self.model_name = model_name 37 | 38 | def init(self, high_level_skillset: SkillSet, low_level_skillset: SkillSet, vision_skill: VisionSkillWrapper): 39 | self.high_level_skillset = high_level_skillset 40 | self.low_level_skillset = low_level_skillset 41 | self.vision_skill = vision_skill 42 | 43 | def plan(self, task_description: str, scene_description: Optional[str] = None, error_message: Optional[str] = None, execution_history: Optional[str] = None): 44 | # by default, the task_description is an action 45 | if not task_description.startswith("["): 46 | task_description = "[A] " + task_description 47 | 48 | if scene_description is None: 49 | scene_description = self.vision_skill.get_obj_list() 50 | prompt = self.prompt_plan.format(system_skill_description_high=self.high_level_skillset, 51 | system_skill_description_low=self.low_level_skillset, 52 | guides=self.guides, 53 | plan_examples=self.plan_examples, 54 | error_message=error_message, 55 | scene_description=scene_description, 56 | task_description=task_description, 57 | execution_history=execution_history) 58 | print_t(f"[P] Planning request: {task_description}") 59 | return self.llm.request(prompt, self.model_name, stream=False) 60 | 61 | def probe(self, question: str) -> MiniSpecValueType: 62 | prompt = self.prompt_probe.format(scene_description=self.vision_skill.get_obj_list(), question=question) 63 | print_t(f"[P] Execution request: {question}") 64 | return evaluate_value(self.llm.request(prompt, self.model_name)), False -------------------------------------------------------------------------------- /controller/llm_wrapper.py: -------------------------------------------------------------------------------- 1 | import os 2 | import openai 3 | from openai import Stream, ChatCompletion 4 | 5 | GPT3 = "gpt-3.5-turbo-16k" 6 | GPT4 = "gpt-4" 7 | LLAMA3 = "meta-llama/Meta-Llama-3-8B-Instruct" 8 | 9 | CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) 10 | chat_log_path = os.path.join(CURRENT_DIR, "assets/chat_log.txt") 11 | 12 | class LLMWrapper: 13 | def __init__(self, temperature=0.0): 14 | self.temperature = temperature 15 | self.llama_client = openai.OpenAI( 16 | # base_url="http://10.66.41.78:8000/v1", 17 | base_url="http://localhost:8000/v1", 18 | api_key="token-abc123", 19 | ) 20 | self.gpt_client = openai.OpenAI( 21 | api_key=os.environ.get("OPENAI_API_KEY"), 22 | ) 23 | 24 | def request(self, prompt, model_name=GPT4, stream=False) -> str | Stream[ChatCompletion.ChatCompletionChunk]: 25 | if model_name == LLAMA3: 26 | client = self.llama_client 27 | else: 28 | client = self.gpt_client 29 | 30 | response = client.chat.completions.create( 31 | model=model_name, 32 | messages=[{"role": "user", "content": prompt}], 33 | temperature=self.temperature, 34 | stream=stream, 35 | ) 36 | 37 | # save the message in a txt 38 | with open(chat_log_path, "a") as f: 39 | f.write(prompt + "\n---\n") 40 | if not stream: 41 | f.write(response.model_dump_json(indent=2) + "\n---\n") 42 | 43 | if stream: 44 | return response 45 | 46 | return response.choices[0].message.content -------------------------------------------------------------------------------- /controller/minispec_interpreter.py: -------------------------------------------------------------------------------- 1 | from typing import List, Tuple, Union 2 | import re, queue 3 | from enum import Enum 4 | import time 5 | from typing import Optional 6 | from threading import Thread 7 | from queue import Queue 8 | from openai import ChatCompletion, Stream 9 | from .skillset import SkillSet 10 | from .utils import split_args, print_t 11 | 12 | 13 | def print_debug(*args): 14 | print(*args) 15 | # pass 16 | 17 | MiniSpecValueType = Union[int, float, bool, str, None] 18 | 19 | def evaluate_value(value: str) -> MiniSpecValueType: 20 | if value.isdigit(): 21 | return int(value) 22 | elif value.replace('.', '', 1).isdigit(): 23 | return float(value) 24 | elif value == 'True': 25 | return True 26 | elif value == 'False': 27 | return False 28 | elif value == 'None' or len(value) == 0: 29 | return None 30 | else: 31 | return value.strip('\'"') 32 | 33 | class MiniSpecReturnValue: 34 | def __init__(self, value: MiniSpecValueType, replan: bool): 35 | self.value = value 36 | self.replan = replan 37 | 38 | def from_tuple(t: Tuple[MiniSpecValueType, bool]): 39 | return MiniSpecReturnValue(t[0], t[1]) 40 | 41 | def default(): 42 | return MiniSpecReturnValue(None, False) 43 | 44 | def __repr__(self) -> str: 45 | return f'value={self.value}, replan={self.replan}' 46 | 47 | class ParsingState(Enum): 48 | CODE = 0 49 | ARGUMENTS = 1 50 | CONDITION = 2 51 | LOOP_COUNT = 3 52 | SUB_STATEMENTS = 4 53 | 54 | class MiniSpecProgram: 55 | def __init__(self, env: Optional[dict] = None, mq: queue.Queue = None) -> None: 56 | self.statements: List[Statement] = [] 57 | self.depth = 0 58 | self.finished = False 59 | self.ret = False 60 | if env is None: 61 | self.env = {} 62 | else: 63 | self.env = env 64 | self.current_statement = Statement(self.env) 65 | self.mq = mq 66 | 67 | def parse(self, code_instance: Stream[ChatCompletion.ChatCompletionChunk] | List[str], exec: bool = False) -> bool: 68 | for chunk in code_instance: 69 | if isinstance(chunk, str): 70 | code = chunk 71 | else: 72 | code = chunk.choices[0].delta.content 73 | if code == None or len(code) == 0: 74 | continue 75 | if self.mq: 76 | self.mq.put(code + '\\\\') 77 | for c in code: 78 | if self.current_statement.parse(c, exec): 79 | if len(self.current_statement.action) > 0: 80 | print_debug("Adding statement: ", self.current_statement, exec) 81 | self.statements.append(self.current_statement) 82 | self.current_statement = Statement(self.env) 83 | if c == '{': 84 | self.depth += 1 85 | elif c == '}': 86 | if self.depth == 0: 87 | self.finished = True 88 | return True 89 | self.depth -= 1 90 | return False 91 | 92 | def eval(self) -> MiniSpecReturnValue: 93 | print_debug(f'Eval program: {self}, finished: {self.finished}') 94 | ret_val = MiniSpecReturnValue.default() 95 | count = 0 96 | while not self.finished: 97 | if len(self.statements) <= count: 98 | time.sleep(0.1) 99 | continue 100 | ret_val = self.statements[count].eval() 101 | if ret_val.replan or self.statements[count].ret: 102 | print_debug(f'RET from {self.statements[count]} with {ret_val} {self.statements[count].ret}') 103 | self.ret = True 104 | return ret_val 105 | count += 1 106 | if count < len(self.statements): 107 | for i in range(count, len(self.statements)): 108 | ret_val = self.statements[i].eval() 109 | if ret_val.replan or self.statements[i].ret: 110 | print_debug(f'RET from {self.statements[i]} with {ret_val} {self.statements[i].ret}') 111 | self.ret = True 112 | return ret_val 113 | return ret_val 114 | 115 | def __repr__(self) -> str: 116 | s = '' 117 | for statement in self.statements: 118 | s += f'{statement}; ' 119 | return s 120 | 121 | class Statement: 122 | execution_queue: Queue['Statement'] = None 123 | low_level_skillset: SkillSet = None 124 | high_level_skillset: SkillSet = None 125 | def __init__(self, env: dict) -> None: 126 | self.code_buffer: str = '' 127 | self.parsing_state: ParsingState = ParsingState.CODE 128 | self.condition: Optional[str] = None 129 | self.loop_count: Optional[int] = None 130 | self.action: str = '' 131 | self.allow_digit: bool = False 132 | self.executable: bool = False 133 | self.ret: bool = False 134 | self.sub_statements: Optional[MiniSpecProgram] = None 135 | self.env = env 136 | self.read_argument: bool = False 137 | 138 | def get_env_value(self, var) -> MiniSpecValueType: 139 | if var not in self.env: 140 | raise Exception(f'Variable {var} is not defined') 141 | return self.env[var] 142 | 143 | def parse(self, code: str, exec: bool = False) -> bool: 144 | for c in code: 145 | match self.parsing_state: 146 | case ParsingState.CODE: 147 | if c == '?' and not self.read_argument: 148 | self.action = 'if' 149 | self.parsing_state = ParsingState.CONDITION 150 | elif c == ';' or c == '}' or c == ')': 151 | if c == ')': 152 | self.code_buffer += c 153 | self.read_argument = False 154 | self.action = self.code_buffer 155 | print_debug(f'SP Action: {self.code_buffer}') 156 | self.executable = True 157 | if exec and self.action != '': 158 | self.execution_queue.put(self) 159 | return True 160 | else: 161 | if c == '(': 162 | self.read_argument = True 163 | if c.isalpha() or c == '_': 164 | self.allow_digit = True 165 | self.code_buffer += c 166 | if c.isdigit() and not self.allow_digit: 167 | self.action = 'loop' 168 | self.parsing_state = ParsingState.LOOP_COUNT 169 | case ParsingState.CONDITION: 170 | if c == '{': 171 | print_debug(f'SP Condition: {self.code_buffer}') 172 | self.condition = self.code_buffer 173 | self.executable = True 174 | if exec: 175 | self.execution_queue.put(self) 176 | self.sub_statements = MiniSpecProgram(self.env) 177 | self.parsing_state = ParsingState.SUB_STATEMENTS 178 | else: 179 | self.code_buffer += c 180 | case ParsingState.LOOP_COUNT: 181 | if c == '{': 182 | print_debug(f'SP Loop: {self.code_buffer}') 183 | self.loop_count = int(self.code_buffer) 184 | self.executable = True 185 | if exec: 186 | self.execution_queue.put(self) 187 | self.sub_statements = MiniSpecProgram(self.env) 188 | self.parsing_state = ParsingState.SUB_STATEMENTS 189 | else: 190 | self.code_buffer += c 191 | case ParsingState.SUB_STATEMENTS: 192 | if self.sub_statements.parse([c]): 193 | return True 194 | return False 195 | 196 | def eval(self) -> MiniSpecReturnValue: 197 | print_debug(f'Statement eval: {self} {self.action} {self.condition} {self.loop_count}') 198 | while not self.executable: 199 | time.sleep(0.1) 200 | if self.action == 'if': 201 | ret_val = self.eval_condition(self.condition) 202 | if ret_val.replan: 203 | return ret_val 204 | if ret_val.value: 205 | print_debug(f'-> eval condition statement: {self.sub_statements}') 206 | ret_val = self.sub_statements.eval() 207 | if ret_val.replan or self.sub_statements.ret: 208 | self.ret = True 209 | return ret_val 210 | else: 211 | return MiniSpecReturnValue.default() 212 | elif self.action == 'loop': 213 | print_debug(f'-> eval loop statement: {self.loop_count} {self.sub_statements}') 214 | ret_val = MiniSpecReturnValue.default() 215 | for _ in range(self.loop_count): 216 | print_debug(f'-> loop iteration: {ret_val}') 217 | ret_val = self.sub_statements.eval() 218 | if ret_val.replan or self.sub_statements.ret: 219 | self.ret = True 220 | return ret_val 221 | return ret_val 222 | else: 223 | self.ret = False 224 | return self.eval_expr(self.action) 225 | 226 | # def eval_action(self, action: str) -> MiniSpecReturnValue: 227 | # action = action.strip() 228 | # print_debug(f'Eval action: {action}') 229 | 230 | # if '=' in action: 231 | # var, expr = action.split('=') 232 | # print_debug(f'Assignment: Var: {var.strip()}, Val: {expr.strip()}') 233 | # expr = expr.strip() 234 | # ret_val = self.eval_function(expr.strip()) 235 | # if not ret_val.replan: 236 | # self.env[var.strip()] = ret_val.value 237 | # return ret_val 238 | # elif action.startswith('->'): 239 | # self.ret = True 240 | # return self.eval_expr(action.lstrip("->")) 241 | # else: 242 | # return self.eval_function(action) 243 | 244 | def eval_function(self, func: str) -> MiniSpecReturnValue: 245 | print_debug(f'Eval function: {func}') 246 | # append to execution state queue 247 | func = func.split('(', 1) 248 | name = func[0].strip() 249 | if len(func) == 2: 250 | args = func[1].strip()[:-1] 251 | args = split_args(args) 252 | for i in range(0, len(args)): 253 | args[i] = args[i].strip().strip('\'"') 254 | if args[i].startswith('_'): 255 | args[i] = self.get_env_value(args[i]) 256 | else: 257 | args = [] 258 | 259 | if name == 'int': 260 | return MiniSpecReturnValue(int(args[0]), False) 261 | elif name == 'float': 262 | return MiniSpecReturnValue(float(args[0]), False) 263 | elif name == 'str': 264 | return MiniSpecReturnValue(args[0], False) 265 | else: 266 | skill_instance = Statement.low_level_skillset.get_skill(name) 267 | if skill_instance is not None: 268 | print_debug(f'Executing low-level skill: {skill_instance.get_name()} {args}') 269 | return MiniSpecReturnValue.from_tuple(skill_instance.execute(args)) 270 | 271 | skill_instance = Statement.high_level_skillset.get_skill(name) 272 | if skill_instance is not None: 273 | print_debug(f'Executing high-level skill: {skill_instance.get_name()}', args, skill_instance.execute(args)) 274 | interpreter = MiniSpecProgram() 275 | interpreter.parse([skill_instance.execute(args)]) 276 | interpreter.finished = True 277 | val = interpreter.eval() 278 | if val.value == 'rp': 279 | return MiniSpecReturnValue(f'High-level skill {skill_instance.get_name()} failed', True) 280 | return val 281 | raise Exception(f'Skill {name} is not defined') 282 | 283 | def eval_expr(self, var: str) -> MiniSpecReturnValue: 284 | print_t(f'Eval expr: {var}') 285 | var = var.strip() 286 | if var.startswith('->'): 287 | self.ret = True 288 | return MiniSpecReturnValue(self.eval_expr(var.lstrip('->')).value, True) 289 | if '=' in var: 290 | 291 | var, expr = var.split('=') 292 | print_t(f'Eval expr var assign: {var} {expr}') 293 | expr = expr.strip() 294 | ret_val = self.eval_expr(expr) 295 | # if not ret_val.replan: 296 | self.env[var] = ret_val.value 297 | return ret_val 298 | # deal with + - * / operators 299 | if '+' in var or '-' in var or '*' in var or '/' in var: 300 | if '+' in var: 301 | operands = var.split('+') 302 | val = 0 303 | for operand in operands: 304 | val += self.eval_expr(operand).value 305 | elif '-' in var: 306 | operands = var.split('-') 307 | val = self.eval_expr(operands[0]).value 308 | for operand in operands[1:]: 309 | val -= self.eval_expr(operand).value 310 | elif '*' in var: 311 | operands = var.split('*') 312 | val = 1 313 | for operand in operands: 314 | val *= self.eval_expr(operand).value 315 | elif '/' in var: 316 | operands = var.split('/') 317 | val = self.eval_expr(operands[0]).value 318 | for operand in operands[1:]: 319 | val /= self.eval_expr(operand).value 320 | return MiniSpecReturnValue(val, False) 321 | 322 | if len(var) == 0: 323 | raise Exception('Empty operand') 324 | if var.startswith('_'): 325 | return MiniSpecReturnValue(self.get_env_value(var), False) 326 | elif var == 'True' or var == 'False': 327 | return MiniSpecReturnValue(evaluate_value(var), False) 328 | elif var[0].isalpha(): 329 | return self.eval_function(var) 330 | else: 331 | return MiniSpecReturnValue(evaluate_value(var), False) 332 | 333 | def eval_condition(self, condition: str) -> MiniSpecReturnValue: 334 | if '&' in condition: 335 | conditions = condition.split('&') 336 | cond = True 337 | for c in conditions: 338 | ret_val = self.eval_condition(c) 339 | if ret_val.replan: 340 | return ret_val 341 | cond = cond and ret_val.value 342 | return MiniSpecReturnValue(cond, False) 343 | if '|' in condition: 344 | conditions = condition.split('|') 345 | for c in conditions: 346 | ret_val = self.eval_condition(c) 347 | if ret_val.replan: 348 | return ret_val 349 | if ret_val.value == True: 350 | return MiniSpecReturnValue(True, False) 351 | return MiniSpecReturnValue(False, False) 352 | 353 | operand_1, comparator, operand_2 = re.split(r'(>|<|==|!=)', condition) 354 | operand_1 = self.eval_expr(operand_1) 355 | if operand_1.replan: 356 | return operand_1 357 | operand_2 = self.eval_expr(operand_2) 358 | if operand_2.replan: 359 | return operand_2 360 | 361 | print_debug(f'Condition ops: {operand_1.value} {comparator} {operand_2.value}') 362 | if type(operand_1.value) == int and type(operand_2.value) == float or \ 363 | type(operand_1.value) == float and type(operand_2.value) == int: 364 | operand_1.value = float(operand_1.value) 365 | operand_2.value = float(operand_2.value) 366 | 367 | if type(operand_1.value) != type(operand_2.value): 368 | if comparator == '!=': 369 | return MiniSpecReturnValue(True, False) 370 | elif comparator == '==': 371 | return MiniSpecReturnValue(False, False) 372 | else: 373 | raise Exception(f'Invalid comparator: {operand_1.value}:{type(operand_1.value)} {operand_2.value}:{type(operand_2.value)}') 374 | 375 | if comparator == '>': 376 | cmp = operand_1.value > operand_2.value 377 | elif comparator == '<': 378 | cmp = operand_1.value < operand_2.value 379 | elif comparator == '==': 380 | cmp = operand_1.value == operand_2.value 381 | elif comparator == '!=': 382 | cmp = operand_1.value != operand_2.value 383 | else: 384 | raise Exception(f'Invalid comparator: {comparator}') 385 | 386 | return MiniSpecReturnValue(cmp, False) 387 | 388 | def __repr__(self) -> str: 389 | s = '' 390 | if self.action == 'if': 391 | s += f'if {self.condition}' 392 | elif self.action == 'loop': 393 | s += f'[{self.loop_count}]' 394 | else: 395 | s += f'{self.action}' 396 | if self.sub_statements is not None: 397 | s += ' {' 398 | for statement in self.sub_statements.statements: 399 | s += f'{statement}; ' 400 | s += '}' 401 | return s 402 | 403 | class MiniSpecInterpreter: 404 | def __init__(self, message_queue: queue.Queue): 405 | self.env = {} 406 | self.ret = False 407 | self.code_buffer: str = '' 408 | self.execution_history = [] 409 | if Statement.low_level_skillset is None or \ 410 | Statement.high_level_skillset is None: 411 | raise Exception('Statement: Skillset is not initialized') 412 | 413 | Statement.execution_queue = Queue() 414 | self.execution_thread = Thread(target=self.executor) 415 | self.execution_thread.start() 416 | 417 | self.timestamp_get_plan = None 418 | self.timestamp_start_execution = None 419 | self.timestamp_end_execution = None 420 | self.program_count = 0 421 | self.ret_queue = Queue() 422 | self.message_queue = message_queue 423 | 424 | def execute(self, code: Stream[ChatCompletion.ChatCompletionChunk] | List[str]) -> MiniSpecReturnValue: 425 | print_t(f'>>> Get a stream') 426 | self.execution_history = [] 427 | self.timestamp_get_plan = time.time() 428 | program = MiniSpecProgram(mq=self.message_queue) 429 | program.parse(code, True) 430 | self.program_count = len(program.statements) 431 | t2 = time.time() 432 | print_t(">>> Program: ", program, "Time: ", t2 - self.timestamp_get_plan) 433 | 434 | def executor(self): 435 | while True: 436 | if not Statement.execution_queue.empty(): 437 | if self.timestamp_start_execution is None: 438 | self.timestamp_start_execution = time.time() 439 | print_t(">>> Start execution") 440 | statement = Statement.execution_queue.get() 441 | print_debug(f'Queue get statement: {statement}') 442 | ret_val = statement.eval() 443 | print_t(f'Queue statement done: {statement}') 444 | if statement.ret: 445 | while not Statement.execution_queue.empty(): 446 | Statement.execution_queue.get() 447 | self.ret_queue.put(ret_val) 448 | return 449 | self.execution_history.append(statement) 450 | # if ret_val.replan: 451 | # print_t(f'Queue statement replan: {statement}') 452 | # self.ret_queue.put(ret_val) 453 | # return 454 | self.program_count -= 1 455 | if self.program_count == 0: 456 | self.timestamp_end_execution = time.time() 457 | print_t(f'>>> Execution time: {self.timestamp_end_execution - self.timestamp_start_execution}') 458 | self.timestamp_start_execution = None 459 | self.ret_queue.put(ret_val) 460 | return 461 | else: 462 | time.sleep(0.005) 463 | -------------------------------------------------------------------------------- /controller/shared_frame.py: -------------------------------------------------------------------------------- 1 | from PIL import Image 2 | from typing import Optional 3 | from numpy.typing import NDArray 4 | import numpy as np 5 | import threading 6 | import time 7 | 8 | class Frame(): 9 | def __init__(self, image: Image.Image | NDArray[np.uint8]=None, depth: Optional[NDArray[np.int16]]=None): 10 | if image is None: 11 | self._image_buffer = np.zeros((352, 640, 3), dtype=np.uint8) 12 | self._image = Image.fromarray(self._image_buffer) 13 | if isinstance(image, np.ndarray): 14 | self._image_buffer = image 15 | self._image = Image.fromarray(image) 16 | elif isinstance(image, Image.Image): 17 | self._image = image 18 | self._image_buffer = np.array(image) 19 | self._depth = depth 20 | 21 | @property 22 | def image(self) -> Image.Image: 23 | return self._image 24 | 25 | @property 26 | def depth(self) -> Optional[NDArray[np.int16]]: 27 | return self._depth 28 | 29 | @image.setter 30 | def image(self, image: Image.Image): 31 | self._image = image 32 | self._image_buffer = np.array(image) 33 | 34 | @depth.setter 35 | def depth(self, depth: Optional[NDArray[np.int16]]): 36 | self._depth = depth 37 | 38 | @property 39 | def image_buffer(self) -> NDArray[np.uint8]: 40 | return self._image_buffer 41 | 42 | @image_buffer.setter 43 | def image_buffer(self, image_buffer: NDArray[np.uint8]): 44 | self._image_buffer = image_buffer 45 | self._image = Image.fromarray(image_buffer) 46 | 47 | class SharedFrame(): 48 | def __init__(self): 49 | self.timestamp = 0 50 | self.frame = Frame() 51 | self.yolo_result = {} 52 | self.lock = threading.Lock() 53 | 54 | def get_image(self) -> Optional[Image.Image]: 55 | with self.lock: 56 | return self.frame.image 57 | 58 | def get_yolo_result(self) -> dict: 59 | with self.lock: 60 | return self.yolo_result 61 | 62 | def get_depth(self) -> Optional[NDArray[np.int16]]: 63 | with self.lock: 64 | return self.frame.depth 65 | 66 | def set(self, frame: Frame, yolo_result: dict): 67 | with self.lock: 68 | self.frame = frame 69 | self.timestamp = time.time() 70 | self.yolo_result = yolo_result -------------------------------------------------------------------------------- /controller/skillset.py: -------------------------------------------------------------------------------- 1 | import re 2 | from enum import Enum 3 | from typing import Optional, List, Union 4 | from .abs.skill_item import SkillItem, SkillArg 5 | 6 | class SkillSetLevel(Enum): 7 | LOW = "low" 8 | HIGH = "high" 9 | 10 | class SkillSet(): 11 | def __init__(self, level = "low", lower_level_skillset: 'SkillSet' = None): 12 | self.skills = {} 13 | self.level = SkillSetLevel(level) 14 | self.lower_level_skillset = lower_level_skillset 15 | 16 | def get_skill(self, skill_name: str) -> Optional[SkillItem]: 17 | """Returns a SkillItem by its name or abbr.""" 18 | skill = None 19 | if skill_name in self.skills: 20 | skill = self.skills[skill_name] 21 | elif skill_name in SkillItem.abbr_dict: 22 | skill = self.skills.get(SkillItem.abbr_dict[skill_name]) 23 | return skill 24 | 25 | def add_skill(self, skill_item: SkillItem): 26 | """Adds a SkillItem to the set.""" 27 | if skill_item.skill_name in self.skills: 28 | raise ValueError(f"A skill with the name '{skill_item.skill_name}' already exists.") 29 | # Set the low-level skillset for high-level skills 30 | if self.level == SkillSetLevel.HIGH and isinstance(skill_item, HighLevelSkillItem): 31 | if self.lower_level_skillset is not None: 32 | skill_item.set_skillset(self.lower_level_skillset, self) 33 | else: 34 | raise ValueError("Low-level skillset is not set.") 35 | 36 | self.skills[skill_item.skill_name] = skill_item 37 | 38 | def remove_skill(self, skill_name: str): 39 | """Removes a SkillItem from the set by its name.""" 40 | if skill_name not in self.skills: 41 | raise ValueError(f"No skill found with the name '{skill_name}'.") 42 | # remove skill by value 43 | del self.skills[skill_name] 44 | 45 | def __repr__(self) -> str: 46 | string = "" 47 | for skill in self.skills.values(): 48 | string += f"{skill}\n" 49 | return string 50 | 51 | class LowLevelSkillItem(SkillItem): 52 | def __init__(self, skill_name: str, skill_callable: callable, 53 | skill_description: str = "", args: List[SkillArg] = []): 54 | self.skill_name = skill_name 55 | self.abbr = self.generate_abbreviation(skill_name) 56 | self.abbr_dict[self.abbr] = skill_name 57 | self.skill_callable = skill_callable 58 | self.skill_description = skill_description 59 | self.args = args 60 | 61 | def get_name(self) -> str: 62 | return self.skill_name 63 | 64 | def get_skill_description(self) -> str: 65 | return self.skill_description 66 | 67 | def get_argument(self) -> List[SkillArg]: 68 | return self.args 69 | 70 | def execute(self, arg_list: List[Union[int, float, str]]): 71 | """Executes the skill with the provided arguments.""" 72 | if callable(self.skill_callable): 73 | parsed_args = self.parse_args(arg_list) 74 | return self.skill_callable(*parsed_args) 75 | else: 76 | raise ValueError(f"'{self.skill_callable}' is not a callable function.") 77 | 78 | def __repr__(self) -> str: 79 | return (f"abbr:{self.abbr}," 80 | f"name:{self.skill_name}," 81 | f"args:{[arg for arg in self.args]}," 82 | f"description:{self.skill_description}") 83 | 84 | class HighLevelSkillItem(SkillItem): 85 | def __init__(self, skill_name: str, definition: str, 86 | skill_description: str = ""): 87 | self.skill_name = skill_name 88 | self.abbr = self.generate_abbreviation(skill_name) 89 | self.abbr_dict[self.abbr] = skill_name 90 | self.definition = definition 91 | self.skill_description = skill_description 92 | self.low_level_skillset = None 93 | self.args = [] 94 | 95 | def load_from_dict(skill_dict: dict): 96 | return HighLevelSkillItem(skill_dict["skill_name"], skill_dict["definition"], skill_dict["skill_description"]) 97 | 98 | def get_name(self) -> str: 99 | return self.skill_name 100 | 101 | def get_skill_description(self) -> str: 102 | return self.skill_description 103 | 104 | def get_argument(self) -> List[SkillArg]: 105 | return self.args 106 | 107 | def set_skillset(self, low_level_skillset: SkillSet, high_level_skillset: SkillSet): 108 | self.low_level_skillset = low_level_skillset 109 | self.high_level_skillset = high_level_skillset 110 | self.args = self.generate_argument_list() 111 | 112 | def generate_argument_list(self) -> List[SkillArg]: 113 | # Extract all skill calls with their arguments from the code 114 | skill_calls = re.findall(r'(\w+)\(([^)]*)\)', self.definition) 115 | 116 | arg_types = {} 117 | 118 | for skill_name, args in skill_calls: 119 | args = [a.strip() for a in args.split(',')] 120 | if skill_name == "int": 121 | function_args = [SkillArg("value", int)] 122 | elif skill_name == "float": 123 | function_args = [SkillArg("value", float)] 124 | elif skill_name == "str": 125 | function_args = [SkillArg("value", str)] 126 | else: 127 | skill = self.low_level_skillset.get_skill(skill_name) 128 | if skill is None: 129 | skill = self.high_level_skillset.get_skill(skill_name) 130 | 131 | if skill is None: 132 | raise ValueError(f"Skill '{skill_name}' not found in the low-level or high-level skillset.") 133 | 134 | function_args = skill.get_argument() 135 | for i, arg in enumerate(args): 136 | if arg.startswith('$') and arg not in arg_types: 137 | # Match the positional argument with its type from the function definition 138 | arg_types[arg] = function_args[i] 139 | 140 | # Convert the mapped arguments to a user-friendly list in order of $position 141 | arg_types = dict(sorted(arg_types.items())) 142 | arg_list = [arg for arg in arg_types.values()] 143 | 144 | return arg_list 145 | 146 | def execute(self, arg_list: List[Union[int, float, str]]): 147 | """Executes the skill with the provided arguments.""" 148 | if self.low_level_skillset is None: 149 | raise ValueError("Low-level skillset is not set.") 150 | if len(arg_list) != len(self.args): 151 | raise ValueError(f"Expected {len(self.args)} arguments, but got {len(arg_list)}.") 152 | # replace all $1, $2, ... with segments 153 | definition = self.definition 154 | for i in range(0, len(arg_list)): 155 | definition = definition.replace(f"${i + 1}", arg_list[i]) 156 | return definition 157 | 158 | def __repr__(self) -> str: 159 | return (f"abbr:{self.abbr}," 160 | f"name:{self.skill_name}," 161 | f"definition:{self.definition}," 162 | f"args:{[arg for arg in self.args]}," 163 | f"description:{self.skill_description}") -------------------------------------------------------------------------------- /controller/tello_wrapper.py: -------------------------------------------------------------------------------- 1 | import time, cv2 2 | import numpy as np 3 | from typing import Tuple 4 | from djitellopy import Tello 5 | 6 | from .abs.robot_wrapper import RobotWrapper 7 | 8 | import logging 9 | Tello.LOGGER.setLevel(logging.WARNING) 10 | 11 | MOVEMENT_MIN = 20 12 | MOVEMENT_MAX = 300 13 | 14 | SCENE_CHANGE_DISTANCE = 120 15 | SCENE_CHANGE_ANGLE = 90 16 | 17 | def adjust_exposure(img, alpha=1.0, beta=0): 18 | """ 19 | Adjust the exposure of an image. 20 | 21 | :param img: Input image 22 | :param alpha: Contrast control (1.0-3.0). Higher values increase exposure. 23 | :param beta: Brightness control (0-100). Higher values add brightness. 24 | :return: Exposure adjusted image 25 | """ 26 | # Apply exposure adjustment using the formula: new_img = img * alpha + beta 27 | new_img = cv2.convertScaleAbs(img, alpha=alpha, beta=beta) 28 | return new_img 29 | 30 | def sharpen_image(img): 31 | """ 32 | Apply a sharpening filter to an image. 33 | 34 | :param img: Input image 35 | :return: Sharpened image 36 | """ 37 | # Define a sharpening kernel 38 | kernel = np.array([[0, -1, 0], 39 | [-1, 5, -1], 40 | [0, -1, 0]]) 41 | 42 | # Apply the sharpening filter 43 | sharpened = cv2.filter2D(img, -1, kernel) 44 | return sharpened 45 | 46 | class FrameReader: 47 | def __init__(self, fr): 48 | # Initialize the video capture 49 | self.fr = fr 50 | 51 | @property 52 | def frame(self): 53 | # Read a frame from the video capture 54 | frame = self.fr.frame 55 | frame = adjust_exposure(frame, alpha=1.3, beta=-30) 56 | return sharpen_image(frame) 57 | 58 | def cap_distance(distance): 59 | if distance < MOVEMENT_MIN: 60 | return MOVEMENT_MIN 61 | elif distance > MOVEMENT_MAX: 62 | return MOVEMENT_MAX 63 | return distance 64 | 65 | class TelloWrapper(RobotWrapper): 66 | def __init__(self): 67 | self.drone = Tello() 68 | self.active_count = 0 69 | self.stream_on = False 70 | 71 | def keep_active(self): 72 | if self.active_count % 20 == 0: 73 | self.drone.send_control_command("command") 74 | self.active_count += 1 75 | 76 | def connect(self): 77 | self.drone.connect() 78 | 79 | def takeoff(self) -> bool: 80 | if not self.is_battery_good(): 81 | return False 82 | else: 83 | self.drone.takeoff() 84 | return True 85 | 86 | def land(self): 87 | self.drone.land() 88 | 89 | def start_stream(self): 90 | self.stream_on = True 91 | self.drone.streamon() 92 | 93 | def stop_stream(self): 94 | self.stream_on = False 95 | self.drone.streamoff() 96 | 97 | def get_frame_reader(self): 98 | if not self.stream_on: 99 | return None 100 | return FrameReader(self.drone.get_frame_read()) 101 | 102 | def move_forward(self, distance: int) -> Tuple[bool, bool]: 103 | self.drone.move_forward(cap_distance(distance)) 104 | self.movement_x_accumulator += distance 105 | time.sleep(0.5) 106 | return True, distance > SCENE_CHANGE_DISTANCE 107 | 108 | def move_backward(self, distance: int) -> Tuple[bool, bool]: 109 | self.drone.move_back(cap_distance(distance)) 110 | self.movement_x_accumulator -= distance 111 | time.sleep(0.5) 112 | return True, distance > SCENE_CHANGE_DISTANCE 113 | 114 | def move_left(self, distance: int) -> Tuple[bool, bool]: 115 | self.drone.move_left(cap_distance(distance)) 116 | self.movement_y_accumulator += distance 117 | time.sleep(0.5) 118 | return True, distance > SCENE_CHANGE_DISTANCE 119 | 120 | def move_right(self, distance: int) -> Tuple[bool, bool]: 121 | self.drone.move_right(cap_distance(distance)) 122 | self.movement_y_accumulator -= distance 123 | time.sleep(0.5) 124 | return True, distance > SCENE_CHANGE_DISTANCE 125 | 126 | def move_up(self, distance: int) -> Tuple[bool, bool]: 127 | self.drone.move_up(cap_distance(distance)) 128 | time.sleep(0.5) 129 | return True, False 130 | 131 | def move_down(self, distance: int) -> Tuple[bool, bool]: 132 | self.drone.move_down(cap_distance(distance)) 133 | time.sleep(0.5) 134 | return True, False 135 | 136 | def turn_ccw(self, degree: int) -> Tuple[bool, bool]: 137 | self.drone.rotate_counter_clockwise(degree) 138 | self.rotation_accumulator += degree 139 | time.sleep(1) 140 | # return True, degree > SCENE_CHANGE_ANGLE 141 | return True, False 142 | 143 | def turn_cw(self, degree: int) -> Tuple[bool, bool]: 144 | self.drone.rotate_clockwise(degree) 145 | self.rotation_accumulator -= degree 146 | time.sleep(1) 147 | # return True, degree > SCENE_CHANGE_ANGLE 148 | return True, False 149 | 150 | def is_battery_good(self) -> bool: 151 | self.battery = self.drone.query_battery() 152 | print(f"> Battery level: {self.battery}% ", end='') 153 | if self.battery < 20: 154 | print('is too low [WARNING]') 155 | else: 156 | print('[OK]') 157 | return True 158 | return False -------------------------------------------------------------------------------- /controller/utils.py: -------------------------------------------------------------------------------- 1 | from typing import Union 2 | import datetime 3 | 4 | def print_t(*args, **kwargs): 5 | # Get the current timestamp 6 | current_time = datetime.datetime.now().strftime('%H:%M:%S.%f')[:-3] 7 | 8 | # Use built-in print to display the timestamp followed by the message 9 | print(f"[{current_time}]", *args, **kwargs) 10 | 11 | def input_t(literal): 12 | # Get the current timestamp 13 | current_time = datetime.datetime.now().strftime('%H:%M:%S.%f')[:-3] 14 | 15 | # Use built-in print to display the timestamp followed by the message 16 | return input(f"[{current_time}] {literal}") 17 | 18 | def split_args(arg_str: str) -> list[str]: 19 | args = [] 20 | current_arg = '' 21 | parentheses_count = 0 # Keep track of open parentheses 22 | 23 | if arg_str.startswith('\'') and arg_str.endswith('\''): 24 | args.append(arg_str) 25 | return args 26 | 27 | for char in arg_str: 28 | if char == ',' and parentheses_count == 0: 29 | # If we encounter a comma and we're not inside parentheses, split here 30 | args.append(current_arg.strip()) 31 | current_arg = '' 32 | else: 33 | # Otherwise, keep adding characters to the current argument 34 | if char == '(': 35 | parentheses_count += 1 36 | elif char == ')': 37 | parentheses_count -= 1 38 | current_arg += char 39 | 40 | # Don't forget to add the last argument after the loop finishes 41 | if current_arg: 42 | args.append(current_arg.strip()) 43 | 44 | return args -------------------------------------------------------------------------------- /controller/virtual_robot_wrapper.py: -------------------------------------------------------------------------------- 1 | import cv2, time 2 | from typing import Tuple 3 | from .abs.robot_wrapper import RobotWrapper 4 | 5 | class FrameReader: 6 | def __init__(self, cap): 7 | # Initialize the video capture 8 | self.cap = cap 9 | if not self.cap.isOpened(): 10 | raise ValueError("Could not open video device") 11 | 12 | @property 13 | def frame(self): 14 | # Read a frame from the video capture 15 | ret, frame = self.cap.read() 16 | if not ret: 17 | raise ValueError("Could not read frame") 18 | return cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) 19 | 20 | class VirtualRobotWrapper(RobotWrapper): 21 | def __init__(self): 22 | self.stream_on = False 23 | pass 24 | 25 | def keep_active(self): 26 | pass 27 | 28 | def connect(self): 29 | pass 30 | 31 | def takeoff(self) -> bool: 32 | return True 33 | 34 | def land(self): 35 | pass 36 | 37 | def start_stream(self): 38 | self.cap = cv2.VideoCapture(0) 39 | self.stream_on = True 40 | 41 | def stop_stream(self): 42 | self.cap.release() 43 | self.stream_on = False 44 | 45 | def get_frame_reader(self): 46 | if not self.stream_on: 47 | return None 48 | return FrameReader(self.cap) 49 | 50 | def move_forward(self, distance: int) -> Tuple[bool, bool]: 51 | print(f"-> Moving forward {distance} cm") 52 | self.movement_x_accumulator += distance 53 | time.sleep(1) 54 | return True, False 55 | 56 | def move_backward(self, distance: int) -> Tuple[bool, bool]: 57 | print(f"-> Moving backward {distance} cm") 58 | self.movement_x_accumulator -= distance 59 | time.sleep(1) 60 | return True, False 61 | 62 | def move_left(self, distance: int) -> Tuple[bool, bool]: 63 | print(f"-> Moving left {distance} cm") 64 | self.movement_y_accumulator += distance 65 | time.sleep(1) 66 | return True, False 67 | 68 | def move_right(self, distance: int) -> Tuple[bool, bool]: 69 | print(f"-> Moving right {distance} cm") 70 | self.movement_y_accumulator -= distance 71 | time.sleep(1) 72 | return True, False 73 | 74 | def move_up(self, distance: int) -> Tuple[bool, bool]: 75 | print(f"-> Moving up {distance} cm") 76 | time.sleep(1) 77 | return True, False 78 | 79 | def move_down(self, distance: int) -> Tuple[bool, bool]: 80 | print(f"-> Moving down {distance} cm") 81 | time.sleep(1) 82 | return True, False 83 | 84 | def turn_ccw(self, degree: int) -> Tuple[bool, bool]: 85 | print(f"-> Turning CCW {degree} degrees") 86 | self.rotation_accumulator += degree 87 | if degree >= 90: 88 | print("-> Turning CCW over 90 degrees") 89 | return True, False 90 | time.sleep(1) 91 | return True, False 92 | 93 | def turn_cw(self, degree: int) -> Tuple[bool, bool]: 94 | print(f"-> Turning CW {degree} degrees") 95 | self.rotation_accumulator -= degree 96 | if degree >= 90: 97 | print("-> Turning CW over 90 degrees") 98 | return True, False 99 | time.sleep(1) 100 | return True, False -------------------------------------------------------------------------------- /controller/vision_skill_wrapper.py: -------------------------------------------------------------------------------- 1 | from typing import Union, Tuple, Optional 2 | import numpy as np 3 | import time, math 4 | import cv2 5 | from filterpy.kalman import KalmanFilter 6 | from .shared_frame import SharedFrame 7 | 8 | def iou(boxA, boxB): 9 | # Calculate the intersection over union (IoU) of two bounding boxes 10 | xA = max(boxA['x1'], boxB['x1']) 11 | yA = max(boxA['y1'], boxB['y1']) 12 | xB = min(boxA['x2'], boxB['x2']) 13 | yB = min(boxA['y2'], boxB['y2']) 14 | 15 | # Compute the area of intersection rectangle 16 | interArea = max(0, xB - xA) * max(0, yB - yA) 17 | 18 | # Compute the area of both the prediction and ground-truth rectangles 19 | boxAArea = (boxA['x2'] - boxA['x1']) * (boxA['y2'] - boxA['y1']) 20 | boxBArea = (boxB['x2'] - boxB['x1']) * (boxB['y2'] - boxB['y1']) 21 | 22 | # Compute the intersection over union 23 | iou = interArea / float(boxAArea + boxBArea - interArea) 24 | 25 | return iou 26 | 27 | def euclidean_distance(boxA, boxB): 28 | centerA = ((boxA['x1'] + boxA['x2']) / 2, (boxA['y1'] + boxA['y2']) / 2) 29 | centerB = ((boxB['x1'] + boxB['x2']) / 2, (boxB['y1'] + boxB['y2']) / 2) 30 | return math.sqrt((centerA[0] - centerB[0])**2 + (centerA[1] - centerB[1])**2) 31 | 32 | 33 | class ObjectInfo: 34 | def __init__(self, name, x, y, w, h) -> None: 35 | self.name = name 36 | self.x = float(x) 37 | self.y = float(y) 38 | self.w = float(w) 39 | self.h = float(h) 40 | 41 | def __str__(self) -> str: 42 | return f"{self.name} x:{self.x:.2f} y:{self.y:.2f} width:{self.w:.2f} height:{self.h:.2f}" 43 | 44 | class ObjectTracker: 45 | def __init__(self, name, x, y, w, h) -> None: 46 | self.name = name 47 | self.kf_pos = self.init_filter() 48 | self.kf_siz = self.init_filter() 49 | self.timestamp = 0 50 | self.size = None 51 | self.update(x, y, w, h) 52 | 53 | def update(self, x, y, w, h): 54 | self.kf_pos.update((x, y)) 55 | self.kf_siz.update((w, h)) 56 | self.timestamp = time.time() 57 | 58 | def predict(self) -> Optional[ObjectInfo]: 59 | # if no update in 2 seconds, return None 60 | if time.time() - self.timestamp > 0.5: 61 | return None 62 | self.kf_pos.predict() 63 | self.kf_siz.predict() 64 | return ObjectInfo(self.name, self.kf_pos.x[0][0], self.kf_pos.x[1][0], self.kf_siz.x[0][0], self.kf_siz.x[1][0]) 65 | 66 | def init_filter(self): 67 | kf = KalmanFilter(dim_x=4, dim_z=2) # 4 state dimensions (x, y, vx, vy), 2 measurement dimensions (x, y) 68 | kf.F = np.array([[1, 0, 1, 0], # State transition matrix 69 | [0, 1, 0, 1], 70 | [0, 0, 1, 0], 71 | [0, 0, 0, 1]]) 72 | kf.H = np.array([[1, 0, 0, 0], # Measurement function 73 | [0, 1, 0, 0]]) 74 | kf.R *= 1 # Measurement uncertainty 75 | kf.P *= 1000 # Initial uncertainty 76 | kf.Q *= 0.01 # Process uncertainty 77 | return kf 78 | 79 | class VisionSkillWrapper(): 80 | def __init__(self, shared_frame: SharedFrame): 81 | self.shared_frame = shared_frame 82 | self.last_update = 0 83 | self.object_trackers: dict[str, ObjectTracker] = {} 84 | self.object_list = [] 85 | self.aruco_detector = cv2.aruco.ArucoDetector( 86 | cv2.aruco.getPredefinedDictionary(cv2.aruco.DICT_4X4_250), 87 | cv2.aruco.DetectorParameters()) 88 | 89 | def update(self): 90 | if self.shared_frame.timestamp == self.last_update: 91 | return 92 | self.last_update = self.shared_frame.timestamp 93 | self.object_list = [] 94 | objs = self.shared_frame.get_yolo_result()['result'] 95 | for obj in objs: 96 | name = obj['name'] 97 | box = obj['box'] 98 | x = (box['x1'] + box['x2']) / 2 99 | y = (box['y1'] + box['y2']) / 2 100 | w = box['x2'] - box['x1'] 101 | h = box['y2'] - box['y1'] 102 | self.object_list.append(ObjectInfo(name, x, y, w, h)) 103 | def _update(self): 104 | if self.shared_frame.timestamp == self.last_update: 105 | return 106 | self.last_update = self.shared_frame.timestamp 107 | 108 | objs = self.shared_frame.get_yolo_result()['result'] 109 | 110 | updated_trackers = {} 111 | 112 | for obj in objs: 113 | name = obj['name'] 114 | box = obj['box'] 115 | x = (box['x1'] + box['x2']) / 2 116 | y = (box['y1'] + box['y2']) / 2 117 | w = box['x2'] - box['x1'] 118 | h = box['y2'] - box['y1'] 119 | 120 | best_match_key = None 121 | best_match_distance = float('inf') 122 | 123 | # Find the best matching tracker 124 | for key, tracker in self.object_trackers.items(): 125 | if tracker.name == name: 126 | existing_box = { 127 | 'x1': tracker.kf_pos.x[0][0] - tracker.kf_siz.x[0][0] / 2, 128 | 'y1': tracker.kf_pos.x[1][0] - tracker.kf_siz.x[1][0] / 2, 129 | 'x2': tracker.kf_pos.x[0][0] + tracker.kf_siz.x[0][0] / 2, 130 | 'y2': tracker.kf_pos.x[1][0] + tracker.kf_siz.x[1][0] / 2, 131 | } 132 | distance = euclidean_distance(existing_box, box) 133 | if distance < best_match_distance: 134 | best_match_distance = distance 135 | best_match_key = key 136 | 137 | # Update the best matching tracker or create a new one 138 | if best_match_key is not None and best_match_distance < 50: # Threshold can be adjusted 139 | self.object_trackers[best_match_key].update(x, y, w, h) 140 | updated_trackers[best_match_key] = self.object_trackers[best_match_key] 141 | else: 142 | new_key = f"{name}_{len(self.object_trackers)}" # Create a unique key 143 | updated_trackers[new_key] = ObjectTracker(name, x, y, w, h) 144 | 145 | # Replace the old trackers with the updated ones 146 | self.object_trackers = updated_trackers 147 | 148 | # Create the list of current objects 149 | self.object_list = [] 150 | to_delete = [] 151 | for key, tracker in self.object_trackers.items(): 152 | obj = tracker.predict() 153 | if obj is not None: 154 | self.object_list.append(obj) 155 | else: 156 | to_delete.append(key) 157 | 158 | # Remove trackers that should be deleted 159 | for key in to_delete: 160 | del self.object_trackers[key] 161 | # def update(self): 162 | # if self.shared_frame.timestamp == self.last_update: 163 | # return 164 | # self.last_update = self.shared_frame.timestamp 165 | # objs = self.shared_frame.get_yolo_result()['result'] + self.shared_frame.get_yolo_result()['result_custom'] 166 | # for obj in objs: 167 | # name = obj['name'] 168 | # box = obj['box'] 169 | # x = (box['x1'] + box['x2']) / 2 170 | # y = (box['y1'] + box['y2']) / 2 171 | # w = box['x2'] - box['x1'] 172 | # h = box['y2'] - box['y1'] 173 | # if name not in self.object_trackers: 174 | # self.object_trackers[name] = ObjectTracker(name, x, y, w, h) 175 | # else: 176 | # self.object_trackers[name].update(x, y, w, h) 177 | 178 | # self.object_list = [] 179 | # to_delete = [] 180 | # for name, tracker in self.object_trackers.items(): 181 | # obj = tracker.predict() 182 | # if obj is not None: 183 | # self.object_list.append(obj) 184 | # else: 185 | # to_delete.append(name) 186 | # for name in to_delete: 187 | # del self.object_trackers[name] 188 | 189 | def get_obj_list(self) -> str: 190 | self.update() 191 | str_list = [] 192 | for obj in self.object_list: 193 | str_list.append(str(obj)) 194 | return str(str_list).replace("'", '') 195 | 196 | def get_obj_info(self, object_name: str) -> ObjectInfo: 197 | for _ in range(10): 198 | self.update() 199 | for obj in self.object_list: 200 | if obj.name.startswith(object_name): 201 | return obj 202 | time.sleep(0.2) 203 | return None 204 | 205 | def is_visible(self, object_name: str) -> Tuple[bool, bool]: 206 | return self.get_obj_info(object_name) is not None, False 207 | 208 | def object_x(self, object_name: str) -> Tuple[Union[float, str], bool]: 209 | info = self.get_obj_info(object_name) 210 | if info is None: 211 | return f'object_x: {object_name} is not in sight', True 212 | return info.x, False 213 | 214 | def object_y(self, object_name: str) -> Tuple[Union[float, str], bool]: 215 | info = self.get_obj_info(object_name) 216 | if info is None: 217 | return f'object_y: {object_name} is not in sight', True 218 | return info.y, False 219 | 220 | def object_width(self, object_name: str) -> Tuple[Union[float, str], bool]: 221 | info = self.get_obj_info(object_name) 222 | if info is None: 223 | return f'object_width: {object_name} not in sight', True 224 | return info.w, False 225 | 226 | def object_height(self, object_name: str) -> Tuple[Union[float, str], bool]: 227 | info = self.get_obj_info(object_name) 228 | if info is None: 229 | return f'object_height: {object_name} not in sight', True 230 | return info.h, False 231 | 232 | def object_distance(self, object_name: str) -> Tuple[Union[int, str], bool]: 233 | info = self.get_obj_info(object_name) 234 | if info is None: 235 | return f'object_distance: {object_name} not in sight', True 236 | mid_point = (info.x, info.y) 237 | FOV_X = 0.42 238 | FOV_Y = 0.55 239 | if mid_point[0] < 0.5 - FOV_X / 2 or mid_point[0] > 0.5 + FOV_X / 2 \ 240 | or mid_point[1] < 0.5 - FOV_Y / 2 or mid_point[1] > 0.5 + FOV_Y / 2: 241 | return 30, False 242 | depth = self.shared_frame.get_depth().data 243 | start_x = 0.5 - FOV_X / 2 244 | start_y = 0.5 - FOV_Y / 2 245 | index_x = (mid_point[0] - start_x) / FOV_X * (depth.shape[1] - 1) 246 | index_y = (mid_point[1] - start_y) / FOV_Y * (depth.shape[0] - 1) 247 | return int(depth[int(index_y), int(index_x)] / 10), False -------------------------------------------------------------------------------- /controller/yolo_client.py: -------------------------------------------------------------------------------- 1 | from io import BytesIO 2 | from PIL import Image, ImageDraw, ImageFont 3 | from typing import Optional, Tuple 4 | from numpy.typing import NDArray 5 | import numpy as np 6 | from contextlib import asynccontextmanager 7 | 8 | import json, os 9 | import requests 10 | import queue 11 | import asyncio, aiohttp 12 | import threading 13 | 14 | from .utils import print_t 15 | from .shared_frame import SharedFrame, Frame 16 | 17 | DIR = os.path.dirname(os.path.abspath(__file__)) 18 | 19 | VISION_SERVICE_IP = os.environ.get("VISION_SERVICE_IP", "localhost") 20 | ROUTER_SERVICE_PORT = os.environ.get("ROUTER_SERVICE_PORT", "50049") 21 | 22 | ''' 23 | Access the YOLO service through http. 24 | ''' 25 | class YoloClient(): 26 | def __init__(self, shared_frame: SharedFrame=None): 27 | self.service_url = 'http://{}:{}/yolo'.format(VISION_SERVICE_IP, ROUTER_SERVICE_PORT) 28 | self.image_size = (640, 352) 29 | self.frame_queue = queue.Queue() # queue element: (frame_id, frame) 30 | self.shared_frame = shared_frame 31 | self.frame_id = 0 32 | self.frame_id_lock = asyncio.Lock() 33 | 34 | def is_local_service(self): 35 | return VISION_SERVICE_IP == 'localhost' 36 | 37 | def image_to_bytes(image): 38 | # compress and convert the image to bytes 39 | imgByteArr = BytesIO() 40 | image.save(imgByteArr, format='WEBP') 41 | return imgByteArr.getvalue() 42 | 43 | def plot_results(frame, results): 44 | if results is None: 45 | return 46 | def str_float_to_int(value, multiplier): 47 | return int(float(value) * multiplier) 48 | draw = ImageDraw.Draw(frame) 49 | font = ImageFont.truetype(os.path.join(DIR, "assets/Roboto-Medium.ttf"), size=50) 50 | w, h = frame.size 51 | for result in results: 52 | box = result["box"] 53 | draw.rectangle((str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h), str_float_to_int(box["x2"], w), str_float_to_int(box["y2"], h)), 54 | fill=None, outline='blue', width=4) 55 | draw.text((str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h) - 50), result["name"], fill='red', font=font) 56 | 57 | def plot_results_oi(frame, object_list): 58 | if object_list is None or len(object_list) == 0: 59 | return 60 | def str_float_to_int(value, multiplier): 61 | return int(float(value) * multiplier) 62 | draw = ImageDraw.Draw(frame) 63 | font = ImageFont.truetype(os.path.join(DIR, "assets/Roboto-Medium.ttf"), size=50) 64 | w, h = frame.size 65 | for obj in object_list: 66 | draw.rectangle((str_float_to_int(obj.x - obj.w / 2, w), str_float_to_int(obj.y - obj.h / 2, h), str_float_to_int(obj.x + obj.w / 2, w), str_float_to_int(obj.y + obj.h / 2, h)), 67 | fill=None, outline='blue', width=4) 68 | draw.text((str_float_to_int(obj.x - obj.w / 2, w), str_float_to_int(obj.y - obj.h / 2, h) - 50), obj.name, fill='red', font=font) 69 | 70 | def retrieve(self) -> Optional[SharedFrame]: 71 | return self.shared_frame 72 | 73 | @asynccontextmanager 74 | async def get_aiohttp_session_response(service_url, data, timeout_seconds=3): 75 | timeout = aiohttp.ClientTimeout(total=timeout_seconds) 76 | try: 77 | # The ClientSession now uses the defined timeout 78 | async with aiohttp.ClientSession(timeout=timeout) as session: 79 | async with session.post(service_url, data=data) as response: 80 | response.raise_for_status() # Optional: raises exception for 4XX/5XX responses 81 | yield response 82 | except aiohttp.ServerTimeoutError: 83 | print_t(f"[Y] Timeout error when connecting to {service_url}") 84 | 85 | def detect_local(self, frame: Frame, conf=0.2): 86 | image = frame.image 87 | image_bytes = YoloClient.image_to_bytes(image.resize(self.image_size)) 88 | self.frame_queue.put(frame) 89 | 90 | config = { 91 | 'user_name': 'yolo', 92 | 'stream_mode': True, 93 | 'image_id': self.image_id, 94 | 'conf': conf 95 | } 96 | files = { 97 | 'image': ('image', image_bytes), 98 | 'json_data': (None, json.dumps(config)) 99 | } 100 | 101 | print_t(f"[Y] Sending request to {self.service_url}") 102 | 103 | response = requests.post(self.service_url, files=files) 104 | print_t(f"[Y] Response: {response.text}") 105 | json_results = json.loads(response.text) 106 | if self.shared_frame is not None: 107 | self.shared_frame.set(self.frame_queue.get(), json_results) 108 | 109 | async def detect(self, frame: Frame, conf=0.3): 110 | if self.is_local_service(): 111 | self.detect_local(frame, conf) 112 | return 113 | image = frame.image 114 | image_bytes = YoloClient.image_to_bytes(image.resize(self.image_size)) 115 | 116 | async with self.frame_id_lock: 117 | self.frame_queue.put((self.frame_id, frame)) 118 | config = { 119 | 'user_name': 'yolo', 120 | 'stream_mode': True, 121 | 'image_id': self.image_id, 122 | 'conf': conf 123 | } 124 | files = { 125 | 'image': image_bytes, 126 | 'json_data': json.dumps(config) 127 | } 128 | self.frame_id += 1 129 | 130 | async with YoloClient.get_aiohttp_session_response(self.service_url, files) as response: 131 | results = await response.text() 132 | 133 | try: 134 | json_results = json.loads(results) 135 | except: 136 | print_t(f"[Y] Invalid json results: {results}") 137 | return 138 | 139 | # discard old images 140 | if self.frame_queue.empty(): 141 | return 142 | while self.frame_queue.queue[0][0] < json_results['image_id']: 143 | self.frame_queue.get() 144 | # discard old results 145 | if self.frame_queue.queue[0][0] > json_results['image_id']: 146 | return 147 | 148 | if self.shared_frame is not None: 149 | self.shared_frame.set(self.frame_queue.get()[1], json_results) -------------------------------------------------------------------------------- /controller/yolo_grpc_client.py: -------------------------------------------------------------------------------- 1 | from io import BytesIO 2 | from PIL import Image 3 | from typing import Optional, List 4 | 5 | import json, sys, os 6 | import queue 7 | import grpc 8 | import asyncio 9 | 10 | from .yolo_client import SharedFrame, Frame 11 | from .utils import print_t 12 | 13 | PARENT_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 14 | 15 | DEFAULT_YOLO_LIST = ['person', 'bicycle', 'car', 'motorcycle', 'airplane', 'bus', 'train', 'truck', 'boat', 'traffic light', 'fire hydrant', 'stop sign', 'parking meter', 'bench', 'bird', 'cat', 'dog', 'horse', 'sheep', 'cow', 'elephant', 'bear', 'zebra', 'giraffe', 'backpack', 'umbrella', 'handbag', 'tie', 'suitcase', 'frisbee', 'skis', 'snowboard', 'sports ball', 'kite', 'baseball bat', 'baseball glove', 'skateboard', 'surfboard', 'tennis racket', 'bottle', 'wine glass', 'cup', 'fork', 'knife', 'spoon', 'bowl', 'banana', 'apple', 'sandwich', 'orange', 'broccoli', 'carrot', 'hot dog', 'pizza', 'donut', 'cake', 'chair', 'couch', 'potted plant', 'bed', 'dining table', 'toilet', 'tv', 'laptop', 'mouse', 'remote', 'keyboard', 'cell phone', 'microwave', 'oven', 'toaster', 'sink', 'refrigerator', 'book', 'clock', 'vase', 'scissors', 'teddy bear', 'hair drier', 'toothbrush'] 16 | 17 | sys.path.append(os.path.join(PARENT_DIR, "proto/generated")) 18 | import hyrch_serving_pb2 19 | import hyrch_serving_pb2_grpc 20 | 21 | VISION_SERVICE_IP = os.environ.get("VISION_SERVICE_IP", "localhost") 22 | YOLO_SERVICE_PORT = os.environ.get("YOLO_SERVICE_PORT", "50050").split(",")[0] 23 | 24 | ''' 25 | Access the YOLO service through gRPC. 26 | ''' 27 | class YoloGRPCClient(): 28 | def __init__(self, shared_frame: SharedFrame=None): 29 | channel = grpc.insecure_channel(f'{VISION_SERVICE_IP}:{YOLO_SERVICE_PORT}') 30 | self.stub = hyrch_serving_pb2_grpc.YoloServiceStub(channel) 31 | self.is_async_inited = False 32 | self.image_size = (640, 352) 33 | self.frame_queue = queue.Queue() 34 | self.shared_frame = shared_frame 35 | self.frame_id_lock = asyncio.Lock() 36 | self.frame_id = 0 37 | 38 | def init_async_channel(self): 39 | channel_async = grpc.aio.insecure_channel(f'{VISION_SERVICE_IP}:{YOLO_SERVICE_PORT}') 40 | self.stub_async = hyrch_serving_pb2_grpc.YoloServiceStub(channel_async) 41 | self.is_async_inited = True 42 | 43 | def is_local_service(self): 44 | return VISION_SERVICE_IP == 'localhost' 45 | 46 | def image_to_bytes(image): 47 | # compress and convert the image to bytes 48 | imgByteArr = BytesIO() 49 | image.save(imgByteArr, format='WEBP') 50 | return imgByteArr.getvalue() 51 | 52 | def retrieve(self) -> Optional[SharedFrame]: 53 | return self.shared_frame 54 | 55 | def detect_local(self, frame: Frame, conf=0.2): 56 | image = frame.image 57 | image_bytes = YoloGRPCClient.image_to_bytes(image.resize(self.image_size)) 58 | self.frame_queue.put(frame) 59 | 60 | detect_request = hyrch_serving_pb2.DetectRequest(image_data=image_bytes, conf=conf) 61 | response = self.stub.DetectStream(detect_request) 62 | 63 | json_results = json.loads(response.json_data) 64 | if self.shared_frame is not None: 65 | self.shared_frame.set(self.frame_queue.get(), json_results) 66 | 67 | async def detect(self, frame: Frame, conf=0.1): 68 | if not self.is_async_inited: 69 | self.init_async_channel() 70 | 71 | if self.is_local_service(): 72 | self.detect_local(frame, conf) 73 | return 74 | 75 | image = frame.image 76 | # do not resize for demo 77 | image_bytes = YoloGRPCClient.image_to_bytes(image) 78 | async with self.frame_id_lock: 79 | image_id = self.frame_id 80 | self.frame_queue.put((self.frame_id, frame)) 81 | self.frame_id += 1 82 | 83 | detect_request = hyrch_serving_pb2.DetectRequest(image_id=image_id, image_data=image_bytes, conf=conf) 84 | response = await self.stub_async.Detect(detect_request) 85 | 86 | json_results = json.loads(response.json_data) 87 | if self.frame_queue.empty(): 88 | return 89 | # discard old images 90 | while self.frame_queue.queue[0][0] < json_results['image_id']: 91 | self.frame_queue.get() 92 | # discard old results 93 | if self.frame_queue.queue[0][0] > json_results['image_id']: 94 | return 95 | if self.shared_frame is not None: 96 | self.shared_frame.set(self.frame_queue.get()[1], json_results) -------------------------------------------------------------------------------- /docker/env.list: -------------------------------------------------------------------------------- 1 | ROOT_PATH=/workspace 2 | ROUTER_SERVICE_PORT=50049 3 | YOLO_SERVICE_PORT=50050, 50051, 50052 -------------------------------------------------------------------------------- /docker/router/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM python:3.11-slim-bullseye 2 | RUN apt update 3 | RUN apt install -y nano wget 4 | RUN pip install grpcio-tools quart 5 | 6 | # copy the contents of the current project to /workspace 7 | COPY ../.. /workspace 8 | 9 | # set the working directory to /workspace 10 | WORKDIR /workspace 11 | 12 | # generate the python files from the proto files 13 | RUN cd /workspace/proto && bash ./generate.sh 14 | 15 | CMD ["python", "./serving/router/router.py"] -------------------------------------------------------------------------------- /docker/yolo/Dockerfile: -------------------------------------------------------------------------------- 1 | FROM ultralytics/ultralytics:latest 2 | RUN apt update 3 | RUN apt install -y nano wget 4 | RUN pip install grpcio-tools lapx 5 | 6 | # copy the contents of the current project to /workspace 7 | COPY ../.. /workspace 8 | 9 | # set the working directory to /workspace 10 | WORKDIR /workspace 11 | 12 | # generate the python files from the proto files 13 | RUN cd /workspace/proto && bash ./generate.sh 14 | 15 | CMD ["python", "./serving/yolo/yolo_service.py"] -------------------------------------------------------------------------------- /proto/generate.sh: -------------------------------------------------------------------------------- 1 | python3 -m grpc_tools.protoc -I. --python_out=./generated --grpc_python_out=./generated *.proto -------------------------------------------------------------------------------- /proto/generated/README.md: -------------------------------------------------------------------------------- 1 | "pb2" files will be generated here. -------------------------------------------------------------------------------- /proto/generated/__init__.py: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/typefly/TypeFly/6f40a91bd3e1dee971090c4d33b707c2a8dc5045/proto/generated/__init__.py -------------------------------------------------------------------------------- /proto/hyrch_serving.proto: -------------------------------------------------------------------------------- 1 | syntax = "proto3"; 2 | 3 | service YoloService { 4 | rpc DetectStream (DetectRequest) returns (DetectResponse) {} 5 | rpc Detect (DetectRequest) returns (DetectResponse) {} 6 | } 7 | 8 | message DetectRequest { 9 | optional int32 image_id = 1; 10 | bytes image_data = 2; // Encoded image data 11 | float conf = 3; 12 | } 13 | 14 | message DetectResponse { 15 | string json_data = 1; 16 | } 17 | 18 | message SetClassRequest { 19 | repeated string class_names = 1; 20 | } 21 | 22 | message SetClassResponse { 23 | string result = 1; 24 | } 25 | 26 | service Llama2Service { 27 | rpc ChatRequest (PromptRequest) returns (PromptResponse) {} 28 | } 29 | 30 | message PromptRequest { 31 | string json_data = 1; // prompt 32 | optional bytes image_data = 2; // Encoded image data 33 | } 34 | 35 | message PromptResponse { 36 | string json_data = 1; // response 37 | } 38 | 39 | service LlavaService { 40 | rpc VisionRequest (PromptRequest) returns (PromptResponse) {} 41 | } 42 | -------------------------------------------------------------------------------- /serving/router/router.py: -------------------------------------------------------------------------------- 1 | import sys, os, json 2 | from quart import Quart, request, jsonify 3 | import asyncio 4 | 5 | PARENT_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 6 | ROOT_PATH = os.environ.get("ROOT_PATH", PARENT_DIR) 7 | ROUTER_SERVICE_PORT = os.environ.get("ROUTER_SERVICE_PORT", "50049") 8 | 9 | sys.path.append(os.path.join(ROOT_PATH, "proto/generated")) 10 | import hyrch_serving_pb2 11 | import hyrch_serving_pb2_grpc 12 | 13 | from service_manager import ServiceManager 14 | 15 | app = Quart(__name__) 16 | 17 | grpcServiceManager = ServiceManager() 18 | 19 | service_lock = asyncio.Lock() 20 | 21 | @app.before_serving 22 | async def before_serving(): 23 | global grpcServiceManager 24 | grpcServiceManager.add_service("yolo", os.environ.get("VISION_SERVICE_IPS", "localhost"), os.environ.get("YOLO_SERVICE_PORT", "50050, 50051")) 25 | await grpcServiceManager._initialize_channels() 26 | 27 | @app.route('/yolo', methods=['POST']) 28 | async def process_yolo(): 29 | global grpcServiceManager 30 | global service_lock 31 | files = await request.files 32 | form = await request.form 33 | image_data = files.get('image') 34 | json_str = form.get('json_data') 35 | 36 | print(f"Received request with json_data: {json_str}") 37 | 38 | if not json_str: 39 | return "No JSON data provided", 400 40 | 41 | if not image_data: 42 | return "No image provided", 400 43 | 44 | json_data = json.loads(json_str) 45 | user_name = json_data.get("user_name", "user") 46 | stream_mode = json_data.get("stream_mode", False) 47 | image_id = json_data.get("image_id", None) 48 | conf = json_data.get("conf", 0.2) 49 | 50 | async with service_lock: 51 | channel = await grpcServiceManager.get_service_channel("yolo", dedicated=stream_mode, user_name=user_name) 52 | 53 | try: 54 | stub = hyrch_serving_pb2_grpc.YoloServiceStub(channel) 55 | image_contents = image_data.read() 56 | if stream_mode: 57 | response = await stub.DetectStream(hyrch_serving_pb2.DetectRequest(image_id=image_id, image_data=image_contents, conf=conf)) 58 | else: 59 | response = await stub.Detect(hyrch_serving_pb2.DetectRequest(image_id=image_id, image_data=image_contents, conf=conf)) 60 | finally: 61 | if not stream_mode: 62 | await grpcServiceManager.release_service_channel("yolo", channel) 63 | return response.json_data 64 | 65 | if __name__ == "__main__": 66 | app.run(debug=True, host='0.0.0.0', port=ROUTER_SERVICE_PORT) -------------------------------------------------------------------------------- /serving/router/service_manager.py: -------------------------------------------------------------------------------- 1 | import grpc 2 | import asyncio 3 | import time 4 | 5 | class ServiceManager: 6 | def __init__(self): 7 | self.services = {} 8 | self.channel_queues = {} 9 | self.channels_initialized = False 10 | self.dedicated_channels = {} 11 | self.dedicated_channels_timeout = 10 12 | self.last_cleanup = time.time() 13 | 14 | def add_service(self, service_name, host, ports): 15 | self.services[service_name] = (host, ports.split(",")) 16 | self.channel_queues[service_name] = asyncio.Queue() 17 | 18 | async def _initialize_channels(self): 19 | if self.channels_initialized: 20 | return 21 | for service_name, (host, ports) in self.services.items(): 22 | queue = self.channel_queues[service_name] 23 | for port in ports: 24 | channel = grpc.aio.insecure_channel(f"{host}:{port}") 25 | await queue.put(channel) 26 | self.channels_initialized = True 27 | 28 | async def clean_dedicated_channels(self): 29 | if time.time() - self.last_cleanup > self.dedicated_channels_timeout: 30 | self.last_cleanup = time.time() 31 | 32 | users_to_remove = [] 33 | services_to_remove = [] 34 | 35 | # Collect items to remove 36 | for user_name, service_channels in self.dedicated_channels.items(): 37 | print(f"Checking dedicated channels for user {user_name}") 38 | for service_name, (channel, timestamp) in service_channels.items(): 39 | if time.time() - timestamp > self.dedicated_channels_timeout: 40 | print(f"Removing expired dedicated channel for user {user_name} and service {service_name}") 41 | await self.channel_queues[service_name].put(channel) 42 | services_to_remove.append((user_name, service_name)) 43 | 44 | if len(service_channels) == len([service for user, service in services_to_remove if user == user_name]): 45 | users_to_remove.append(user_name) 46 | 47 | # Perform deletions 48 | for user, service in services_to_remove: 49 | del self.dedicated_channels[user][service] 50 | if len(self.dedicated_channels[user]) == 0: 51 | del self.dedicated_channels[user] 52 | 53 | for user in users_to_remove: 54 | if user in self.dedicated_channels: 55 | del self.dedicated_channels[user] 56 | 57 | async def get_service_channel(self, service_name, dedicated=False, user_name=None): 58 | await self._initialize_channels() # Ensure channels are initialized 59 | 60 | await self.clean_dedicated_channels() 61 | 62 | if dedicated: 63 | # If there is no dedicated channel for this user, get one from the queue 64 | if user_name not in self.dedicated_channels: 65 | # If there is no more channel in the queue, return None 66 | if self.channel_queues[service_name].qsize() > 0: 67 | self.dedicated_channels[user_name] = {service_name: (self.channel_queues[service_name].get_nowait(), time.time())} 68 | else: 69 | return None 70 | # If there is a dedicated channel for this user, update time and return it 71 | else: 72 | self.dedicated_channels[user_name][service_name] = (self.dedicated_channels[user_name][service_name][0], time.time()) 73 | return self.dedicated_channels[user_name][service_name][0] 74 | else: 75 | channel = await self.channel_queues[service_name].get() 76 | return channel 77 | 78 | async def release_service_channel(self, service_name, channel): 79 | await self.channel_queues[service_name].put(channel) -------------------------------------------------------------------------------- /serving/webui/drone-pov.html: -------------------------------------------------------------------------------- 1 |

Drone POV

2 |
3 | drone-pov 4 |
-------------------------------------------------------------------------------- /serving/webui/header.html: -------------------------------------------------------------------------------- 1 |

🪽 TypeFly: Power the Drone with Large Language Model

-------------------------------------------------------------------------------- /serving/webui/install_requirements.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | 3 | # Define a list of required packages 4 | REQUIRED_PKG=("flask" "gradio" "grpcio-tools" "aiohttp" "djitellopy" "openai" "opencv-python" "numpy" "pillow" "filterpy" "matplotlib" "torch") 5 | 6 | # Function to check and install package 7 | check_and_install() { 8 | package=$1 9 | if ! pip3 list | grep -F $package > /dev/null; then 10 | echo "Package $package is not installed. Installing..." 11 | pip3 install $package 12 | else 13 | echo "Package $package is already installed." 14 | fi 15 | } 16 | 17 | # Iterate over required packages and check each one 18 | for pkg in "${REQUIRED_PKG[@]}"; do 19 | check_and_install $pkg 20 | done 21 | 22 | if [ -z "${OPENAI_API_KEY}" ]; then 23 | echo "WARNNING: OPENAI_API_KEY is not set" 24 | fi -------------------------------------------------------------------------------- /serving/webui/typefly.py: -------------------------------------------------------------------------------- 1 | import queue 2 | import sys, os 3 | import asyncio 4 | import io, time 5 | import gradio as gr 6 | from flask import Flask, Response 7 | from threading import Thread 8 | import argparse 9 | 10 | PARENT_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 11 | 12 | sys.path.append(PARENT_DIR) 13 | from controller.llm_controller import LLMController 14 | from controller.utils import print_t 15 | from controller.llm_wrapper import GPT4, LLAMA3 16 | from controller.abs.robot_wrapper import RobotType 17 | 18 | CURRENT_DIR = os.path.dirname(os.path.abspath(__file__)) 19 | 20 | class TypeFly: 21 | def __init__(self, robot_type, use_http=False): 22 | # create a cache folder 23 | self.cache_folder = os.path.join(CURRENT_DIR, 'cache') 24 | if not os.path.exists(self.cache_folder): 25 | os.makedirs(self.cache_folder) 26 | self.message_queue = queue.Queue() 27 | self.message_queue.put(self.cache_folder) 28 | self.llm_controller = LLMController(robot_type, use_http, self.message_queue) 29 | self.system_stop = False 30 | self.ui = gr.Blocks(title="TypeFly") 31 | self.asyncio_loop = asyncio.get_event_loop() 32 | self.use_llama3 = False 33 | default_sentences = [ 34 | "Find something I can eat.", 35 | "What you can see?", 36 | "Follow that ball for 20 seconds", 37 | "Find a chair for me.", 38 | "Go to the chair without book.", 39 | ] 40 | with self.ui: 41 | gr.HTML(open(os.path.join(CURRENT_DIR, 'header.html'), 'r').read()) 42 | gr.HTML(open(os.path.join(CURRENT_DIR, 'drone-pov.html'), 'r').read()) 43 | gr.ChatInterface(self.process_message, retry_btn=None, fill_height=False, examples=default_sentences).queue() 44 | # TODO: Add checkbox to switch between llama3 and gpt4 45 | # gr.Checkbox(label='Use llama3', value=False).select(self.checkbox_llama3) 46 | 47 | def checkbox_llama3(self): 48 | self.use_llama3 = not self.use_llama3 49 | if self.use_llama3: 50 | print_t(f"Switch to llama3") 51 | self.llm_controller.planner.set_model(LLAMA3) 52 | else: 53 | print_t(f"Switch to gpt4") 54 | self.llm_controller.planner.set_model(GPT4) 55 | 56 | def process_message(self, message, history): 57 | print_t(f"[S] Receiving task description: {message}") 58 | if message == "exit": 59 | self.llm_controller.stop_controller() 60 | self.system_stop = True 61 | yield "Shutting down..." 62 | elif len(message) == 0: 63 | return "[WARNING] Empty command!]" 64 | else: 65 | task_thread = Thread(target=self.llm_controller.execute_task_description, args=(message,)) 66 | task_thread.start() 67 | complete_response = '' 68 | while True: 69 | msg = self.message_queue.get() 70 | if isinstance(msg, tuple): 71 | # history.append((message, complete_response)) 72 | history.append((None, msg)) 73 | # complete_response = '' 74 | elif isinstance(msg, str): 75 | if msg == 'end': 76 | # Indicate end of the task to Gradio chat 77 | return "Command Complete!" 78 | 79 | if msg.startswith('[LOG]'): 80 | complete_response += '\n' 81 | if msg.endswith('\\\\'): 82 | complete_response += msg.rstrip('\\\\') 83 | else: 84 | complete_response += msg + '\n' 85 | yield complete_response 86 | 87 | def generate_mjpeg_stream(self): 88 | while True: 89 | if self.system_stop: 90 | break 91 | frame = self.llm_controller.get_latest_frame(True) 92 | if frame is None: 93 | continue 94 | buf = io.BytesIO() 95 | frame.save(buf, format='JPEG') 96 | buf.seek(0) 97 | yield (b'--frame\r\n' 98 | b'Content-Type: image/jpeg\r\n\r\n' + buf.read() + b'\r\n') 99 | time.sleep(1.0 / 30.0) 100 | 101 | def run(self): 102 | asyncio_thread = Thread(target=self.asyncio_loop.run_forever) 103 | asyncio_thread.start() 104 | 105 | self.llm_controller.start_robot() 106 | llmc_thread = Thread(target=self.llm_controller.capture_loop, args=(self.asyncio_loop,)) 107 | llmc_thread.start() 108 | 109 | app = Flask(__name__) 110 | @app.route('/drone-pov/') 111 | def video_feed(): 112 | return Response(self.generate_mjpeg_stream(), mimetype='multipart/x-mixed-replace; boundary=frame') 113 | flask_thread = Thread(target=app.run, kwargs={'host': 'localhost', 'port': 50000, 'debug': True, 'use_reloader': False}) 114 | flask_thread.start() 115 | self.ui.launch(show_api=False, server_port=50001, prevent_thread_lock=True) 116 | while True: 117 | time.sleep(1) 118 | if self.system_stop: 119 | break 120 | 121 | llmc_thread.join() 122 | asyncio_thread.join() 123 | 124 | self.llm_controller.stop_robot() 125 | 126 | # clean self.cache_folder 127 | for file in os.listdir(self.cache_folder): 128 | os.remove(os.path.join(self.cache_folder, file)) 129 | 130 | if __name__ == "__main__": 131 | parser = argparse.ArgumentParser() 132 | parser.add_argument('--use_virtual_robot', action='store_true') 133 | parser.add_argument('--use_http', action='store_true') 134 | parser.add_argument('--gear', action='store_true') 135 | 136 | args = parser.parse_args() 137 | robot_type = RobotType.TELLO 138 | if args.use_virtual_robot: 139 | robot_type = RobotType.VIRTUAL 140 | elif args.gear: 141 | robot_type = RobotType.GEAR 142 | typefly = TypeFly(robot_type, use_http=args.use_http) 143 | typefly.run() -------------------------------------------------------------------------------- /serving/yolo/yolo_service.py: -------------------------------------------------------------------------------- 1 | import sys, os, gc 2 | from concurrent import futures 3 | from PIL import Image 4 | from io import BytesIO 5 | import json 6 | import grpc 7 | import torch 8 | from ultralytics import YOLO 9 | import multiprocessing 10 | 11 | PARENT_DIR = os.path.dirname(os.path.dirname(os.path.dirname(os.path.abspath(__file__)))) 12 | 13 | ROOT_PATH = os.environ.get("ROOT_PATH", PARENT_DIR) 14 | SERVICE_PORT = os.environ.get("YOLO_SERVICE_PORT", "50050, 50051").split(",") 15 | 16 | MODEL_PATH = os.path.join(ROOT_PATH, "./serving/yolo/models/") 17 | MODEL_TYPE = "yolov8x.pt" 18 | 19 | sys.path.append(ROOT_PATH) 20 | sys.path.append(os.path.join(ROOT_PATH, "proto/generated")) 21 | import hyrch_serving_pb2 22 | import hyrch_serving_pb2_grpc 23 | 24 | def load_model(): 25 | model = YOLO(MODEL_PATH + MODEL_TYPE) 26 | if torch.cuda.is_available(): 27 | device = torch.device('cuda:0') 28 | else: 29 | device = torch.device('cpu') 30 | model.to(device) 31 | print(f"GPU memory usage: {torch.cuda.memory_allocated()}") 32 | return model 33 | 34 | def release_model(model): 35 | del model 36 | gc.collect() 37 | torch.cuda.empty_cache() 38 | 39 | """ 40 | gRPC service class. 41 | """ 42 | class YoloService(hyrch_serving_pb2_grpc.YoloServiceServicer): 43 | def __init__(self, port): 44 | self.stream_mode = False 45 | self.model = load_model() 46 | self.port = port 47 | 48 | def reload_model(self): 49 | if self.model is not None: 50 | release_model(self.model) 51 | self.model = load_model() 52 | 53 | @staticmethod 54 | def bytes_to_image(image_bytes): 55 | return Image.open(BytesIO(image_bytes)) 56 | 57 | @staticmethod 58 | def format_result(yolo_result): 59 | if yolo_result.probs is not None: 60 | print('Warning: Classify task do not support `tojson` yet.') 61 | return 62 | formatted_result = [] 63 | data = yolo_result.boxes.data.cpu().tolist() 64 | h, w = yolo_result.orig_shape 65 | for i, row in enumerate(data): # xyxy, track_id if tracking, conf, class_id 66 | box = {'x1': round(row[0] / w, 2), 'y1': round(row[1] / h, 2), 'x2': round(row[2] / w, 2), 'y2': round(row[3] / h, 2)} 67 | conf = row[-2] 68 | class_id = int(row[-1]) 69 | 70 | name = yolo_result.names[class_id] 71 | if yolo_result.boxes.is_track: 72 | # result['track_id'] = int(row[-3]) # track ID 73 | name = f'{name}_{int(row[-3])}' 74 | result = {'name': name, 'confidence': round(conf, 2), 'box': box} 75 | 76 | if yolo_result.masks: 77 | x, y = yolo_result.masks.xy[i][:, 0], yolo_result.masks.xy[i][:, 1] # numpy array 78 | result['segments'] = {'x': (x / w).tolist(), 'y': (y / h).tolist()} 79 | if yolo_result.keypoints is not None: 80 | x, y, visible = yolo_result.keypoints[i].data[0].cpu().unbind(dim=1) # torch Tensor 81 | result['keypoints'] = {'x': (x / w).tolist(), 'y': (y / h).tolist(), 'visible': visible.tolist()} 82 | formatted_result.append(result) 83 | return formatted_result 84 | 85 | def process_image(self, image, id=None, conf=0.3): 86 | if self.stream_mode: 87 | yolo_result = self.model.track(image, verbose=False, conf=conf, tracker="bytetrack.yaml")[0] 88 | else: 89 | yolo_result = self.model(image, verbose=False, conf=conf)[0] 90 | result = { 91 | "image_id": id, 92 | "result": YoloService.format_result(yolo_result), 93 | } 94 | return json.dumps(result) 95 | 96 | def DetectStream(self, request, context): 97 | print(f"Received DetectStream request from {context.peer()} on port {self.port}, image_id: {request.image_id}") 98 | if not self.stream_mode: 99 | self.stream_mode = True 100 | self.reload_model() 101 | 102 | image = YoloService.bytes_to_image(request.image_data) 103 | return hyrch_serving_pb2.DetectResponse(json_data=self.process_image(image, request.image_id, request.conf)) 104 | 105 | def Detect(self, request, context): 106 | print(f"Received Detect request from {context.peer()} on port {self.port}, image_id: {request.image_id}") 107 | if self.stream_mode: 108 | self.stream_mode = False 109 | self.reload_model() 110 | 111 | image = YoloService.bytes_to_image(request.image_data) 112 | return hyrch_serving_pb2.DetectResponse(json_data=self.process_image(image, request.image_id, request.conf)) 113 | 114 | def serve(port): 115 | print(f"Starting YoloService at port {port}") 116 | server = grpc.server(futures.ThreadPoolExecutor(max_workers=1)) 117 | hyrch_serving_pb2_grpc.add_YoloServiceServicer_to_server(YoloService(port), server) 118 | server.add_insecure_port(f'[::]:{port}') 119 | server.start() 120 | server.wait_for_termination() 121 | 122 | if __name__ == '__main__': 123 | # Create a pool of processes 124 | process_count = len(SERVICE_PORT) 125 | processes = [] 126 | 127 | for i in range(process_count): 128 | process = multiprocessing.Process(target=serve, args=(SERVICE_PORT[i],)) 129 | process.start() 130 | processes.append(process) 131 | 132 | # Wait for all processes to complete 133 | for process in processes: 134 | process.join() -------------------------------------------------------------------------------- /test/aiohttp-client.py: -------------------------------------------------------------------------------- 1 | import aiohttp 2 | import json 3 | import asyncio 4 | async def detect(): 5 | image = open('../test/images/kitchen.webp', 'rb') 6 | files = { 7 | 'image': image, 8 | 'json_data': json.dumps({'service': 'yolo'}) 9 | } 10 | print(files) 11 | async with aiohttp.ClientSession() as session: 12 | print("Sending request") 13 | async with session.post("http://0.0.0.0:50048/yolo", data=files) as response: 14 | content = await response.text() 15 | print("Received response") 16 | print(content) 17 | 18 | if __name__ == "__main__": 19 | asyncio.run(detect()) -------------------------------------------------------------------------------- /test/gpt-latency-measurement.py: -------------------------------------------------------------------------------- 1 | import sys, time 2 | # import tiktoken 3 | # sys.path.append("..") 4 | # from controller.llm_wrapper import LLMWrapper 5 | 6 | # llm = LLMWrapper() 7 | # enc = tiktoken.encoding_for_model("gpt-4") 8 | 9 | # def prompt_output_measure(length): 10 | # prompt = 'Please generate the exact same output as the following text: ' 11 | # for i in range(length // 2): 12 | # prompt += str(i % 10) + " " 13 | # return prompt 14 | 15 | # def prompt_input_measure(length): 16 | # suffix = "Please ignore all the above text and just generate True" 17 | # prompt = '' 18 | # init_len = enc.encode(suffix) 19 | # for i in range((length - len(init_len)) // 2): 20 | # prompt += str(i % 10) + " " 21 | # return prompt + suffix 22 | 23 | lengths = [8000] 24 | # lengths = [50, 500, 1000, 2000, 3000, 4000, 5000, 6000, 7000, 8000] 25 | # lengths = [50, 100, 200, 300, 400] 26 | result = [] 27 | # for length in lengths: 28 | # t = 0 29 | # input_length = 0 30 | # output_length = 0 31 | # for i in range(10): 32 | # # prompt = prompt_output_measure(length) 33 | # prompt = prompt_input_measure(length) 34 | # start = time.time() 35 | # input_length += len(enc.encode(prompt)) 36 | # output = llm.request(prompt) 37 | # output_length += len(enc.encode(output)) 38 | # t += time.time() - start 39 | # print(f"t: {t}, i: {input_length}, o: {output_length}") 40 | # print("Time taken for length", length, ":", t / 10) 41 | # print("Input length:", input_length / 10) 42 | # print("Output length:", output_length / 10) 43 | # result.append((length, t / 10, input_length / 10, output_length / 10)) 44 | # print(result) 45 | # exit(0) 46 | 47 | # different output 48 | data_1 = [ 49 | (50, 2.276, 62, 49), 50 | (100, 4.560, 112, 99), 51 | (200, 8.473, 212, 199), 52 | (300, 10.996, 312, 295), 53 | (400, 14.425, 412, 413), 54 | ] 55 | 56 | # different input 57 | # data_2 = [ 58 | # (50, 0.47491774559020994, 49.0, 1.0), 59 | # (100, 0.4766784429550171, 99.0, 1.0), 60 | # (200, 0.46629860401153567, 199.0, 1.0), 61 | # (300, 0.4480326175689697, 299.0, 1.0), 62 | # (400, 0.5139770269393921, 399.0, 1.0), 63 | # (1000, 0.4809334516525269, 999.0, 1.0), 64 | # (2000, 0.6343598604202271, 1999.0, 1.0), 65 | # (4000, 0.7674200057983398, 3999.0, 1.0), 66 | # (8000, 1.3128541946411132, 7999.0, 1.0)] 67 | data_2 = [(50, 0.5378251791000366, 49.0, 1.0), 68 | (500, 0.5108302307128907, 499.0, 1.0), 69 | (1000, 0.4951801300048828, 999.0, 1.0), 70 | (2000, 0.5111032485961914, 1999.0, 1.0), 71 | (3000, 0.5264493227005005, 2999.0, 1.0), 72 | (4000, 0.5382437705993652, 3999.0, 1.0), 73 | (5000, 0.5212562322616577, 4999.0, 1.0), 74 | (6000, 0.5919422626495361, 5999.0, 1.0), 75 | (7000, 0.5916801214218139, 6999.0, 1.0), 76 | (8000, 0.6088189125061035, 7999.0, 1.0)] 77 | 78 | network_latency = 28.127 79 | 80 | red_color = '#FF6B6B' 81 | blue_color = '#4D96FF' 82 | white_color = '#FFFFFF' 83 | black_color = '#000000' 84 | 85 | import matplotlib.pyplot as plt 86 | import numpy as np 87 | from scipy import stats 88 | 89 | col1_1 = [x[0] for x in data_2] 90 | col2 = [x[1] for x in data_2] 91 | 92 | col1_2 = [x[0] for x in data_1] 93 | col3 = [x[1] for x in data_1] 94 | 95 | # Perform linear regression for each dataset 96 | slope2, intercept2, r_value2, p_value2, std_err2 = stats.linregress(col1_1, col2) 97 | 98 | for i in range(len(data_1)): 99 | col3[i] -= data_1[i][2] * slope2 100 | 101 | slope3, intercept3, r_value3, p_value3, std_err3 = stats.linregress(col1_2, col3) 102 | 103 | # Create arrays from the x-coordinates for line plots 104 | line_x1 = np.linspace(min(col1_1), max(col1_1), 100) # For smoother line plot 105 | line_x2 = np.linspace(min(col1_2), max(col1_2), 100) 106 | 107 | # Create line equations for the plots 108 | line2 = slope2 * line_x1 + intercept2 109 | line3 = slope3 * line_x2 + intercept3 110 | 111 | plt.rcParams.update({'legend.fontsize': 19, 'axes.edgecolor': 'black', 112 | 'axes.linewidth': 2.2, 'font.size': 25}) 113 | 114 | ### plot in a single figure 115 | # fig, ax1 = plt.subplots(figsize=[16, 6]) 116 | # plt.tight_layout(pad=2) 117 | # # Plot the first dataset with its regression 118 | # ax1.scatter(col1_1, col2, color=black_color, label='Various input, fixed output', marker='x', linewidth=3, s=200) 119 | # ax1.plot(np.array(col1_1), slope2 * np.array(col1_1) + intercept2, '-', color=black_color, label=f'a={slope2:.6f}, r={r_value2:.4}', linewidth=3) 120 | # ax1.set_xlabel('Input Token Number', color=black_color) 121 | # ax1.set_ylabel('Time Taken (s)') 122 | # ax1.tick_params(axis='x', labelcolor=black_color) 123 | # ax1.tick_params(axis='y') 124 | 125 | # # Create a second x-axis for the second dataset 126 | # ax2 = ax1.twiny() # Create a second x-axis that shares the same y-axis 127 | # ax2.scatter(col1_2, col3, color=black_color, label='Fixed input, various output', linewidth=3, s=200) 128 | # ax2.plot(np.array(col1_2), slope3 * np.array(col1_2) + intercept3, '--', color=black_color, label=f'b={slope3:.6f}, r={r_value3:.4}', linewidth=3) 129 | # ax2.set_xlabel('Output Token Number', color=black_color) 130 | # ax2.tick_params(axis='x', labelcolor=black_color) 131 | 132 | # # Add legends and show plot 133 | # ax1.legend(loc='lower right', bbox_to_anchor=(1, 0.12)) 134 | # ax2.legend(loc='upper left') 135 | # # plt.show() 136 | 137 | # plt.savefig('gpt4-latency.pdf') 138 | 139 | ### plot in two figures 140 | fig, ax1 = plt.subplots(figsize=[14, 6]) 141 | plt.tight_layout(pad=2) 142 | # Plot the first dataset with its regression 143 | ax1.scatter(col1_1, col2, color=black_color, label='Various input, fixed output', marker='x', linewidth=3, s=200) 144 | ax1.plot(np.array(col1_1), slope2 * np.array(col1_1) + intercept2, '-', color=black_color, label=f'a={slope2:.6f}, r={r_value2:.4}', linewidth=3) 145 | ax1.set_xlabel('Input Token Number', color=black_color) 146 | ax1.set_ylabel('Time Taken (s)') 147 | ax1.tick_params(axis='x', labelcolor=black_color) 148 | ax1.tick_params(axis='y') 149 | 150 | # Add legends and show plot 151 | ax1.legend(loc='upper left') 152 | plt.savefig('gpt4-latency-input.pdf') 153 | # plt.show() 154 | 155 | fig, ax2 = plt.subplots(figsize=[14, 6]) 156 | plt.tight_layout(pad=2) 157 | # Create a second x-axis for the second dataset 158 | ax2.scatter(col1_2, col3, color=black_color, label='Fixed input, various output', linewidth=3, s=200) 159 | ax2.plot(np.array(col1_2), slope3 * np.array(col1_2) + intercept3, '--', color=black_color, label=f'b={slope3:.6f}, r={r_value3:.4}', linewidth=3) 160 | ax2.set_xlabel('Output Token Number', color=black_color) 161 | ax2.set_ylabel('Time Taken (s)') 162 | ax2.tick_params(axis='x', labelcolor=black_color) 163 | ax2.tick_params(axis='y') 164 | 165 | # Add legends and show plot 166 | ax2.legend(loc='upper left') 167 | plt.savefig('gpt4-latency-output.pdf') 168 | # plt.show() 169 | -------------------------------------------------------------------------------- /test/images/kitchen.webp: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/typefly/TypeFly/6f40a91bd3e1dee971090c4d33b707c2a8dc5045/test/images/kitchen.webp -------------------------------------------------------------------------------- /test/interpreter-test.py: -------------------------------------------------------------------------------- 1 | import sys 2 | sys.path.append("..") 3 | from controller.llm_controller import LLMController 4 | from controller.minispec_interpreter import MiniSpecInterpreter, MiniSpecProgram 5 | 6 | controller = LLMController() 7 | 8 | MiniSpecInterpreter.low_level_skillset = controller.low_level_skillset 9 | MiniSpecInterpreter.high_level_skillset = controller.high_level_skillset 10 | interpreter = MiniSpecInterpreter() 11 | 12 | # print(interpreter.execute("8{_1=mr(50);?_1!=False{g('tiger');->True}tc(45)}")) 13 | # print(interpreter.execute("g('person')")) 14 | 15 | # interpreter.execute("8{_1=mr(50);?_1!=False{g('tiger');->True;}tc(45)};") 16 | interpreter.execute('?sa("edible object")!=False{tc(45)}tc(180);') -------------------------------------------------------------------------------- /test/stop-tello.py: -------------------------------------------------------------------------------- 1 | from djitellopy import Tello 2 | 3 | tello = Tello() 4 | tello.connect() 5 | tello.streamoff() 6 | tello.land() -------------------------------------------------------------------------------- /test/tello-test.py: -------------------------------------------------------------------------------- 1 | from djitellopy import Tello 2 | import cv2 3 | 4 | class TelloLLM(): 5 | def __init__(self): 6 | self.drone = Tello() 7 | self.drone.connect() 8 | self.battery = self.drone.query_battery() 9 | 10 | def check_battery(self): 11 | self.battery = self.drone.query_battery() 12 | print(f"> Battery level: {self.battery}% ", end='') 13 | if self.battery < 30: 14 | print('is too low [WARNING]') 15 | else: 16 | print('[OK]') 17 | return True 18 | return False 19 | 20 | def start(self): 21 | if not self.check_battery(): 22 | return 23 | 24 | return 25 | 26 | self.streamOn = True 27 | self.drone.takeoff() 28 | self.drone.move_up(20) 29 | self.drone.streamon() 30 | print("> Application Start") 31 | 32 | frame_read = self.drone.get_frame_read() 33 | 34 | count = 0 35 | while (True): 36 | frame = frame_read.frame 37 | if frame is None: 38 | continue 39 | print("### GET Frame: ", frame.shape) 40 | frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB) 41 | cv2.imwrite(f"./cache/frame_{count}.png", frame) 42 | cv2.imshow("Tello", frame) 43 | key = cv2.waitKey(10) & 0xff 44 | # Press esc to exit 45 | if key == 27: 46 | break 47 | count += 1 48 | self.drone.streamoff() 49 | self.drone.land() 50 | 51 | def main(): 52 | tello = TelloLLM() 53 | tello.start() 54 | 55 | if __name__ == "__main__": 56 | main() 57 | -------------------------------------------------------------------------------- /test/yolo-grpc-test.py: -------------------------------------------------------------------------------- 1 | from io import BytesIO 2 | from PIL import Image 3 | import json, sys, os 4 | import grpc 5 | 6 | def image_to_bytes(image): 7 | # compress and convert the image to bytes 8 | imgByteArr = BytesIO() 9 | image.save(imgByteArr, format='WEBP') 10 | return imgByteArr.getvalue() 11 | 12 | PARENT_DIR = os.path.dirname(os.path.dirname(os.path.abspath(__file__))) 13 | 14 | sys.path.append(os.path.join(PARENT_DIR, "proto/generated")) 15 | import hyrch_serving_pb2 16 | import hyrch_serving_pb2_grpc 17 | 18 | VISION_SERVICE_IP = os.environ.get("VISION_SERVICE_IP", "localhost") 19 | YOLO_SERVICE_PORT = os.environ.get("YOLO_SERVICE_PORT", "50050").split(",")[0] 20 | 21 | channel = grpc.insecure_channel(f'{VISION_SERVICE_IP}:{YOLO_SERVICE_PORT}') 22 | stub = hyrch_serving_pb2_grpc.YoloServiceStub(channel) 23 | 24 | detect_request = hyrch_serving_pb2.DetectRequest(image_data=image_to_bytes(Image.open("./images/kitchen.webp")), conf=0.3) 25 | response = stub.DetectStream(detect_request) 26 | 27 | class_request = hyrch_serving_pb2.SetClassRequest(class_names=['shoes']) 28 | stub.SetClasses(class_request) 29 | 30 | json_results = json.loads(response.json_data) 31 | print(json_results) -------------------------------------------------------------------------------- /test/yolo-test-raw.py: -------------------------------------------------------------------------------- 1 | from ultralytics import YOLOWorld, YOLO 2 | import cv2 3 | 4 | # model = YOLOWorld('yolov8s-worldv2.pt') 5 | model = YOLO('yolov9e.pt') 6 | 7 | def format_result(yolo_result): 8 | if yolo_result.probs is not None: 9 | print('Warning: Classify task do not support `tojson` yet.') 10 | return 11 | formatted_result = [] 12 | data = yolo_result.boxes.data.cpu().tolist() 13 | h, w = yolo_result.orig_shape 14 | for i, row in enumerate(data): # xyxy, track_id if tracking, conf, class_id 15 | box = {'x1': round(row[0] / w, 2), 'y1': round(row[1] / h, 2), 'x2': round(row[2] / w, 2), 'y2': round(row[3] / h, 2)} 16 | conf = row[-2] 17 | class_id = int(row[-1]) 18 | 19 | name = yolo_result.names[class_id] 20 | if yolo_result.boxes.is_track: 21 | # result['track_id'] = int(row[-3]) # track ID 22 | name = f'{name}_{int(row[-3])}' 23 | result = {'name': name, 'confidence': round(conf, 2), 'box': box} 24 | 25 | if yolo_result.masks: 26 | x, y = yolo_result.masks.xy[i][:, 0], yolo_result.masks.xy[i][:, 1] # numpy array 27 | result['segments'] = {'x': (x / w).tolist(), 'y': (y / h).tolist()} 28 | if yolo_result.keypoints is not None: 29 | x, y, visible = yolo_result.keypoints[i].data[0].cpu().unbind(dim=1) # torch Tensor 30 | result['keypoints'] = {'x': (x / w).tolist(), 'y': (y / h).tolist(), 'visible': visible.tolist()} 31 | formatted_result.append(result) 32 | return formatted_result 33 | 34 | def plot_results(frame, results): 35 | if results is None: 36 | return 37 | def str_float_to_int(value, multiplier): 38 | return int(float(value) * multiplier) 39 | w, h = frame.shape[1], frame.shape[0] 40 | for result in results: 41 | box = result["box"] 42 | #draw.rectangle((str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h), str_float_to_int(box["x2"], w), str_float_to_int(box["y2"], h)), 43 | #fill=None, outline='blue', width=4) 44 | cv2.rectangle(frame, (str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h)), (str_float_to_int(box["x2"], w), str_float_to_int(box["y2"], h)), (255, 0, 0), 2) 45 | #draw.text((str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h) - 50), result["name"], fill='red', font=font) 46 | cv2.putText(frame, result["name"], (str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h) - 50), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) 47 | cv2.putText(frame, f'{result["confidence"]:.2f}', (str_float_to_int(box["x1"], w), str_float_to_int(box["y1"], h) - 25), cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 0, 255), 2) 48 | # open camera 49 | cap = cv2.VideoCapture(0) 50 | from filterpy.kalman import KalmanFilter 51 | import numpy as np 52 | def init_filter(): 53 | kf = KalmanFilter(dim_x=4, dim_z=2) # 4 state dimensions (x, y, vx, vy), 2 measurement dimensions (x, y) 54 | kf.F = np.array([[1, 0, 1, 0], # State transition matrix 55 | [0, 1, 0, 1], 56 | [0, 0, 1, 0], 57 | [0, 0, 0, 1]]) 58 | kf.H = np.array([[1, 0, 0, 0], # Measurement function 59 | [0, 1, 0, 0]]) 60 | kf.R *= 1 # Measurement uncertainty 61 | kf.P *= 1000 # Initial uncertainty 62 | kf.Q *= 0.01 # Process uncertainty 63 | return kf 64 | 65 | img = cv2.imread('./cache/frame_2.png') 66 | inference = model(img, conf=0.1) 67 | result = format_result(inference[0]) 68 | plot_results(img, result) 69 | cv2.imshow('frame', img) 70 | cv2.waitKey(0) 71 | 72 | # kf = init_filter() 73 | # while True: 74 | # ret, frame = cap.read() 75 | # if not ret: 76 | # break 77 | # # detect 78 | # inference = model.track(frame, conf=0.1) 79 | # result = format_result(inference[0]) 80 | # # result = format_result(model.track(frame, conf=0.1, persist=True, tracker="bytetrack.yaml")[0]) 81 | # plot_results(frame, result) 82 | # has_person = False 83 | # for item in result: 84 | # if item["name"].startswith('suitcase'): 85 | # has_person = True 86 | # loc_x = (item["box"]["x1"] + item["box"]["x2"]) / 2 87 | # loc_y = (item["box"]["y1"] + item["box"]["y2"]) / 2 88 | # kf.update((loc_x, loc_y)) 89 | 90 | # kf.predict() 91 | 92 | # print(kf.x, frame.shape[0], frame.shape[1]) 93 | # cv2.circle(frame, (int(kf.x[0] * frame.shape[1]), int(kf.x[1] * frame.shape[0])), 5, (0, 255, 0), -1) 94 | # # print(model(frame, conf=0.01)) 95 | # # exit(0) 96 | # # display 97 | # cv2.imshow('frame', frame) 98 | # if cv2.waitKey(1) & 0xFF == ord('q'): 99 | # break -------------------------------------------------------------------------------- /test/yolo-test.py: -------------------------------------------------------------------------------- 1 | import sys 2 | from PIL import Image 3 | sys.path.append("..") 4 | from controller.yolo_grpc_client import YoloGRPCClient 5 | from controller.shared_frame import Frame, SharedFrame 6 | 7 | shared_frame = SharedFrame() 8 | yolo_client = YoloGRPCClient(shared_frame=shared_frame) 9 | frame = Frame(image=Image.open("./images/kitchen.webp")) 10 | yolo_client.detect_local(frame) 11 | print(shared_frame.get_yolo_result()) 12 | --------------------------------------------------------------------------------