├── .gitignore ├── LICENSE ├── README.md ├── h4r_ev3_control ├── CMakeLists.txt ├── ev3_controller_plugins.xml ├── include │ └── h4r_ev3_control │ │ ├── Ev3SensorInterface.h │ │ ├── Ev3Strings.h │ │ ├── FixedBuffer.h │ │ ├── H4REv3Port.h │ │ ├── StringEnum.h │ │ ├── ev3_color_controller.h │ │ ├── ev3_gyro_controller.h │ │ ├── ev3_infrared_controller.h │ │ ├── ev3_touch_controller.h │ │ ├── ev3_ultrasonic_controller.h │ │ └── syshelpers.h ├── package.xml ├── src │ └── h4r_ev3_control │ │ ├── Ev3Strings.cpp │ │ ├── H4REv3Port.cpp │ │ ├── ev3_color_controller.cpp │ │ ├── ev3_gyro_controller.cpp │ │ ├── ev3_infrared_controller.cpp │ │ ├── ev3_touch_controller.cpp │ │ ├── ev3_ultrasonic_controller.cpp │ │ └── syshelpers.cpp └── string_gen │ ├── generateMaps.py │ └── strings.yml ├── h4r_ev3_launch ├── CMakeLists.txt ├── config │ ├── color.yaml │ ├── gyro.yaml │ ├── infrared.yaml │ ├── motors.yaml │ ├── touch.yaml │ └── ultrasonic.yaml ├── launch │ ├── color.launch │ ├── gyro.launch │ ├── infrared.launch │ ├── motors.launch │ ├── touch.launch │ └── ultrasonic.launch └── package.xml ├── h4r_ev3_manager ├── CMakeLists.txt ├── ctrl_cfg │ └── motors.yaml ├── include │ └── h4r_ev3_joint_setup │ │ ├── ev3_joint_settings.h │ │ ├── ev3_joint_settings_exception.h │ │ ├── ev3_joint_settings_interface.h │ │ └── ev3_joint_settings_params.h ├── launch │ └── load.launch ├── package.xml └── src │ └── h4r_ev3_manager │ ├── Ev3HardwareInterface.cpp │ ├── Ev3HardwareInterface.h │ └── ev3_ctrl_node.cpp └── h4r_ev3_msgs ├── CMakeLists.txt ├── msg └── Seek.msg └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Compiled Object files 2 | *.slo 3 | *.lo 4 | *.o 5 | *.obj 6 | 7 | # Precompiled Headers 8 | *.gch 9 | *.pch 10 | 11 | # Compiled Dynamic libraries 12 | *.so 13 | *.dylib 14 | *.dll 15 | 16 | # Fortran module files 17 | *.mod 18 | 19 | # Compiled Static libraries 20 | *.lai 21 | *.la 22 | *.a 23 | *.lib 24 | 25 | # Executables 26 | *.exe 27 | *.out 28 | *.app 29 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # h4r_ev3_ctrl 2 | 3 | :warning: **ATTENTION! THE EV3_MANAGER NODE IS SUPPOSED TO RUN ON THE EV3 BRICK DIRECTLY!** :warning: 4 | 5 | :question: Having problems with my software? Please use the Github [Issues](https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues) features instead of direct emailing, maybe your problem and solving it can be useful for other users too, thank you... :heart: 6 | 7 | Use the image in the instructions - it gives you all except for the launchfiles you should launch on your Linux PC to make the Brick load the controller! 8 | 9 | Instructions to get a working image for your SD Card can be found here: 10 | 11 | http://hacks4ros.github.io/h4r_ev3_ctrl/ 12 | 13 | If you want, [here](https://www.instructables.com/id/ROS-Robot-With-Lego-EV3-and-Docker) is a HowTo using Docker Containers created by Mikael Valot. 14 | 15 | 16 | This repository contains the node for the Lego EV3 for ROS. 17 | 18 | Early Demonstration (Recored some time ago): Driving with ROS Controller Diff Drive 19 | 20 | 21 | [![DiffDrive Controller Demonstration Video](http://img.youtube.com/vi/PqFPvFhTMqk/0.jpg)](http://www.youtube.com/watch?v=PqFPvFhTMqk) 22 | 23 | 24 | -------------------------------------------------------------------------------- /h4r_ev3_control/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(h4r_ev3_control) 3 | 4 | # Load catkin and all dependencies required for this package 5 | find_package(catkin REQUIRED COMPONENTS 6 | controller_interface 7 | hardware_interface 8 | pluginlib 9 | realtime_tools 10 | roscpp 11 | sensor_msgs 12 | h4r_ev3_msgs 13 | ) 14 | 15 | add_definitions(-std=c++11) 16 | 17 | include_directories( 18 | include 19 | ${Boost_INCLUDE_DIR} 20 | ${catkin_INCLUDE_DIRS} 21 | ) 22 | 23 | # Declare catkin package 24 | catkin_package( 25 | CATKIN_DEPENDS controller_interface hardware_interface sensor_msgs realtime_tools h4r_ev3_msgs 26 | INCLUDE_DIRS include 27 | LIBRARIES ev3_control 28 | ) 29 | 30 | add_library(ev3_control 31 | src/${PROJECT_NAME}/Ev3Strings.cpp 32 | src/${PROJECT_NAME}/H4REv3Port.cpp 33 | src/${PROJECT_NAME}/syshelpers.cpp 34 | ) 35 | target_link_libraries(ev3_control ${catkin_LIBRARIES}) 36 | 37 | add_library(ev3_controllers 38 | src/${PROJECT_NAME}/ev3_ultrasonic_controller.cpp include/${PROJECT_NAME}/ev3_ultrasonic_controller.h 39 | src/${PROJECT_NAME}/ev3_gyro_controller.cpp include/${PROJECT_NAME}/ev3_gyro_controller.h 40 | src/${PROJECT_NAME}/ev3_touch_controller.cpp include/${PROJECT_NAME}/ev3_touch_controller.h 41 | src/${PROJECT_NAME}/ev3_color_controller.cpp include/${PROJECT_NAME}/ev3_color_controller.h 42 | src/${PROJECT_NAME}/ev3_infrared_controller.cpp include/${PROJECT_NAME}/ev3_infrared_controller.h 43 | ) 44 | target_link_libraries(ev3_controllers ${catkin_LIBRARIES}) 45 | 46 | 47 | 48 | # Install 49 | install(DIRECTORY include/${PROJECT_NAME}/ 50 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}) 51 | 52 | install(TARGETS ev3_control ev3_controllers 53 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 54 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 55 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 56 | ) 57 | 58 | install(FILES ev3_controller_plugins.xml 59 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}) -------------------------------------------------------------------------------- /h4r_ev3_control/ev3_controller_plugins.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | This controller publishes sonar sensor readings to the appropriate topic according to the selected mode 5 | 6 | 7 | 8 | 9 | 10 | This controller publishes Gyro sensor readings 11 | 12 | 13 | 14 | 15 | 16 | This controller publishes Touch sensor readings 17 | 18 | 19 | 20 | 21 | 22 | This controller publishes Color sensor readings 23 | 24 | 25 | 26 | 27 | This controller publishes infrared sensor readings 28 | 29 | 30 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/Ev3SensorInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (Ev3SensorInterface.h) is part of h4r_ev3_control. 3 | * Date: 10.12.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef EV3SENSORINTERFACE_H_ 24 | #define EV3SENSORINTERFACE_H_ 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | using namespace std; 31 | 32 | namespace ev3_control 33 | { 34 | 35 | class Ev3SensorHandle 36 | { 37 | private: 38 | std::string port_name_; 39 | H4REv3Sensor *sensor_; 40 | public: 41 | 42 | Ev3SensorHandle() 43 | :port_name_() 44 | ,sensor_(0) 45 | {} 46 | 47 | Ev3SensorHandle(const std::string &port_name, H4REv3Sensor *sensor) 48 | :port_name_(port_name) 49 | ,sensor_(sensor) 50 | {} 51 | 52 | 53 | virtual ~Ev3SensorHandle() 54 | {} 55 | 56 | const std::string& getName() const 57 | { 58 | return port_name_; 59 | } 60 | 61 | bool setMode(const std::string &mode) 62 | { 63 | //TODO write mode to file 64 | cout<<"SETMODE_:"<value(index,value); 71 | } 72 | 73 | H4REv3Sensor *getSensor() 74 | { 75 | return sensor_; 76 | } 77 | 78 | Ev3Strings::Ev3DriverName getDriverName() 79 | { 80 | Ev3Strings::Ev3DriverName val; 81 | if(sensor_->getDriverName(val)) 82 | { 83 | return val; 84 | } 85 | else 86 | { 87 | return Ev3Strings::EV3DRIVERNAME_NOT_FOUND; 88 | } 89 | } 90 | 91 | 92 | }; 93 | 94 | /** 95 | * The Ev3SensorInterface 96 | */ 97 | class Ev3SensorInterface : public hardware_interface::HardwareResourceManager< Ev3SensorHandle > {}; 98 | 99 | } /* namespace ev3_control */ 100 | 101 | #endif /* EV3SENSORINTERFACE_H_ */ 102 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/Ev3Strings.h: -------------------------------------------------------------------------------- 1 | 2 | /*WARNING WARNING WARNING WARNING WARNING 3 | *This file is generated by script 4 | *generateMaps.py, to add strings change 5 | *strings.yml and run it again! 6 | *WARNING WARNING WARNING WARNING WARNING 7 | */ 8 | #ifndef EV3STRINGS_H 9 | #define EV3STRINGS_H 10 | #include 11 | namespace ev3_control{ 12 | class Ev3Strings 13 | { 14 | private: 15 | Ev3Strings(){}/*thou shalt never construct this!*/ 16 | public: 17 | typedef enum 18 | { 19 | EV3SWITCH_NOT_FOUND=-1, 20 | EV3SWITCH_ON, 21 | EV3SWITCH_OFF, 22 | }Ev3Switch; 23 | 24 | static StringEnum ev3_switch_conv; 25 | 26 | 27 | typedef enum 28 | { 29 | EV3ULTRASONICMODE_NOT_FOUND=-1, 30 | EV3ULTRASONICMODE_US_DC_IN, 31 | EV3ULTRASONICMODE_US_DIST_CM, 32 | EV3ULTRASONICMODE_US_LISTEN, 33 | EV3ULTRASONICMODE_US_DC_CM, 34 | EV3ULTRASONICMODE_US_SI_IN, 35 | EV3ULTRASONICMODE_US_SI_CM, 36 | EV3ULTRASONICMODE_US_DIST_IN, 37 | }Ev3UltrasonicMode; 38 | 39 | static StringEnum ev3_ultrasonic_mode_conv; 40 | 41 | 42 | typedef enum 43 | { 44 | EV3COLORMODE_NOT_FOUND=-1, 45 | EV3COLORMODE_COL_COLOR, 46 | EV3COLORMODE_COL_AMBIENT, 47 | EV3COLORMODE_COL_REFLECT, 48 | EV3COLORMODE_RGB_RAW, 49 | EV3COLORMODE_COL_CAL, 50 | EV3COLORMODE_REF_RAW, 51 | }Ev3ColorMode; 52 | 53 | static StringEnum ev3_color_mode_conv; 54 | 55 | 56 | typedef enum 57 | { 58 | EV3INFRAREDMODE_NOT_FOUND=-1, 59 | EV3INFRAREDMODE_IR_REMOTE, 60 | EV3INFRAREDMODE_IR_PROX, 61 | EV3INFRAREDMODE_IR_REM_A, 62 | EV3INFRAREDMODE_IR_SEEK, 63 | EV3INFRAREDMODE_IR_S_ALT, 64 | EV3INFRAREDMODE_IR_CAL, 65 | }Ev3InfraredMode; 66 | 67 | static StringEnum ev3_infrared_mode_conv; 68 | 69 | 70 | typedef enum 71 | { 72 | EV3DRIVERNAME_NOT_FOUND=-1, 73 | EV3DRIVERNAME_LEGO_EV3_TOUCH, 74 | EV3DRIVERNAME_LEGO_EV3_COLOR, 75 | EV3DRIVERNAME_LEGO_EV3_US, 76 | EV3DRIVERNAME_LEGO_EV3_GYRO, 77 | EV3DRIVERNAME_LEGO_EV3_IR, 78 | }Ev3DriverName; 79 | 80 | static StringEnum ev3_driver_name_conv; 81 | 82 | 83 | typedef enum 84 | { 85 | EV3POLARITY_NOT_FOUND=-1, 86 | EV3POLARITY_INVERSED, 87 | EV3POLARITY_NORMAL, 88 | }Ev3Polarity; 89 | 90 | static StringEnum ev3_polarity_conv; 91 | 92 | 93 | typedef enum 94 | { 95 | EV3PORTSTATUS_NOT_FOUND=-1, 96 | EV3PORTSTATUS_EV3_UART, 97 | EV3PORTSTATUS_NO_CONNECT, 98 | EV3PORTSTATUS_EV3_ANALOG, 99 | }Ev3PortStatus; 100 | 101 | static StringEnum ev3_port_status_conv; 102 | 103 | 104 | typedef enum 105 | { 106 | EV3GYROMODE_NOT_FOUND=-1, 107 | EV3GYROMODE_GYRO_FAS, 108 | EV3GYROMODE_GYRO_ANG, 109 | EV3GYROMODE_GYRO_G_A, 110 | EV3GYROMODE_GYRO_CAL, 111 | EV3GYROMODE_GYRO_RATE, 112 | }Ev3GyroMode; 113 | 114 | static StringEnum ev3_gyro_mode_conv; 115 | 116 | 117 | typedef enum 118 | { 119 | EV3MOTORCOMMANDS_NOT_FOUND=-1, 120 | EV3MOTORCOMMANDS_RESET, 121 | EV3MOTORCOMMANDS_RUN_TO_REL_POS, 122 | EV3MOTORCOMMANDS_RUN_FOREVER, 123 | EV3MOTORCOMMANDS_RUN_DIRECT, 124 | EV3MOTORCOMMANDS_STOP, 125 | EV3MOTORCOMMANDS_RUN_TIMED, 126 | EV3MOTORCOMMANDS_RUN_TO_ABS_POS, 127 | }Ev3MotorCommands; 128 | 129 | static StringEnum ev3_motor_commands_conv; 130 | 131 | 132 | typedef enum 133 | { 134 | EV3TOUCHMODE_NOT_FOUND=-1, 135 | EV3TOUCHMODE_TOUCH, 136 | }Ev3TouchMode; 137 | 138 | static StringEnum ev3_touch_mode_conv; 139 | 140 | 141 | typedef enum 142 | { 143 | EV3PORTDRIVERS_NOT_FOUND=-1, 144 | EV3PORTDRIVERS_LEGOEV3_INPUT_PORT, 145 | EV3PORTDRIVERS_LEGOEV3_INPUT_OUTPORT, 146 | }Ev3PortDrivers; 147 | 148 | static StringEnum ev3_port_drivers_conv; 149 | 150 | 151 | }; 152 | } 153 | #endif 154 | 155 | 156 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/FixedBuffer.h: -------------------------------------------------------------------------------- 1 | /* 2 | (c) 2013 +++ Filip Stoklas, aka FipS, http://www.4FipS.com +++ 3 | THIS CODE IS FREE - LICENSED UNDER THE MIT LICENSE 4 | ARTICLE URL: http://forums.4fips.com/viewtopic.php?f=3&t=1075 5 | 6 | Some name changes and port to Linux by Christian Holl (http://github.com/Hacks4ROS) 7 | */ 8 | 9 | /* 10 | MIT-License 11 | 12 | Copyright (c) 2013 Filip Stoklas 13 | Copyright (c) 2015 Christian Holl 14 | 15 | Permission is hereby granted, free of charge, to any person obtaining a copy of this software and associated documentation files (the "Software"), 16 | to deal in the Software without restriction, including without limitation the rights to use, copy, modify, merge, publish, distribute, 17 | sublicense, and/or sell copies of the Software, and to permit persons to whom the Software is furnished to do so, 18 | subject to the following conditions: The above copyright notice and this permission notice shall be included in all copies or substantial 19 | portions of the Software. 20 | 21 | THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES 22 | OF MERCHANTABILITY, FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE AUTHORS OR COPYRIGHT HOLDERS BE 23 | LIABLE FOR ANY CLAIM, DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM, OUT OF OR IN 24 | CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE SOFTWARE. 25 | */ 26 | 27 | #ifndef FIXEDBUFFER_H_ 28 | #define FIXEDBUFFER_H_ 29 | 30 | 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | 39 | 40 | namespace FixedBuffer 41 | { 42 | 43 | /// A fixed-size string buffer (typically stack-allocated). 44 | template 45 | class FixedBuffer 46 | { 47 | public: 48 | 49 | /// Constructs from a C string if provided, or sets to "" if NULL passed. 50 | FixedBuffer(const char *cstr = NULL) 51 | { 52 | static_assert(N > 0, "FixedBuffer has zero size!"); 53 | 54 | if(cstr) 55 | { 56 | assert(strlen(cstr) < N && "FixedBuffer too small!"); 57 | strcpy(_data.data(), cstr); 58 | } 59 | else 60 | { 61 | _data[0] = '\0'; 62 | } 63 | } 64 | 65 | /// Constructs from a FixedBuffer of a bigger or equal static size. 66 | template 67 | FixedBuffer(const FixedBuffer &rhs) 68 | { 69 | static_assert(M <= N, "FixedBuffer too small!"); 70 | strcpy(_data.data(), rhs.cstr()); 71 | } 72 | 73 | /// Copies from a C string if provided, or sets to "" if nullptr passed. 74 | FixedBuffer & operator = (const char *cstr) 75 | { 76 | static_assert(N > 0, "FixedBuffer of zero size!"); 77 | 78 | if(cstr) 79 | { 80 | assert(strlen(cstr) < N && "Buffer size too small!"); 81 | strcpy(_data.data(), cstr); 82 | } 83 | else 84 | { 85 | _data[0] = '\0'; 86 | } 87 | 88 | return *this; 89 | } 90 | 91 | 92 | FixedBuffer &operator = (const std::string& str) 93 | { 94 | return operator =(str.c_str()); 95 | } 96 | 97 | /// Copies from a FixedBuffer of a bigger or equal static size. 98 | template 99 | FixedBuffer & operator = (const FixedBuffer &rhs) 100 | { 101 | static_assert(M <= N, "FixedBuffer too small!"); 102 | strcpy(_data.data(), rhs.cstr()); 103 | return *this; 104 | } 105 | 106 | /// Returns a C string (always valid). 107 | const char * c_str() const { return _data.data(); } 108 | 109 | /// Formats a string in a good old printf style. 110 | void format(const char *format, ...) 111 | { 112 | va_list args; 113 | va_start(args, format); 114 | // if truncated, '\0' is automatically appended 115 | vsnprintf(_data.data(), N, format, args); 116 | va_end(args); 117 | } 118 | 119 | private: 120 | 121 | std::array _data; 122 | }; 123 | 124 | 125 | } 126 | 127 | 128 | 129 | #endif /* FIXEDBUFFER_H_ */ 130 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/H4REv3Port.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (H4REv3Port.h) is part of h4r_ev3_control. 3 | * Date: 22.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef H4REV3PORT_H_ 24 | #define H4REV3PORT_H_ 25 | 26 | #include 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | 40 | #include 41 | #include 42 | #include 43 | 44 | namespace ev3_control 45 | { 46 | 47 | class H4REv3Port 48 | { 49 | public: 50 | typedef enum 51 | { 52 | H4REV3PORT_OUT, 53 | H4REV3PORT_IN, 54 | }H4Ev3IoPortType; 55 | 56 | 57 | 58 | class OpenFile 59 | { 60 | public: 61 | typedef enum 62 | { 63 | MODE_RW, 64 | MODE_R, 65 | MODE_W, 66 | }FileMode; 67 | OpenFile() 68 | :ptr(0) 69 | ,connect_id_(0) 70 | {} 71 | int connect_id_; 72 | FileNameBuffer fullpath; 73 | FILE *ptr; 74 | }; 75 | 76 | 77 | 78 | 79 | protected: 80 | 81 | 82 | OpenFile f_driver_name; 83 | 84 | Ev3Strings::Ev3DriverName last_driver_; 85 | int last_driver_check_connected_id_; 86 | 87 | 88 | const std::string port_name_; 89 | bool connected_; 90 | int connect_id_; 91 | 92 | 93 | H4Ev3IoPortType port_type_; 94 | FileNameBuffer sys_port_directory_; 95 | FileNameBuffer sys_device_directory_; 96 | 97 | // FILE* openFile(FileNameBuffer fullpath, FileMode mode); 98 | FILE* get_fileptr_(const char* filename, OpenFile::FileMode mode, OpenFile &file, bool device_dir=true); 99 | 100 | bool getDeviceDirectory(); 101 | bool getPortDirectory(); 102 | 103 | 104 | 105 | 106 | public: 107 | 108 | 109 | /** 110 | * The constructor 111 | * @param port_name port_name 112 | * @param port_type input or output port 113 | */ 114 | H4REv3Port(const std::string &port_name, H4Ev3IoPortType port_type); 115 | ~H4REv3Port(); 116 | 117 | 118 | 119 | 120 | /** 121 | * Check if a device is connected to the port 122 | * @return True if connected 123 | */ 124 | bool isConnected(); 125 | 126 | 127 | /** 128 | * Reads a integer string from a sys file 129 | * @param filename The sys file to be read from (only filename without directory) 130 | * @param[out] value output variable for the value only valid on return true 131 | * @param device_dir True if it should open the file in the device_directory, false it will search the port directory for the file 132 | * @param openFile Storage for file pointer 133 | * @return True if read successful, false otherwise 134 | */ 135 | bool readInt(const char *filename, int &value, OpenFile& openFile, bool device_dir=true); 136 | 137 | /** 138 | * Writes a integer string from a sys file 139 | * @param filename The sys file to be read from (only filename without directory) 140 | * @param value The value to be written to the file 141 | * @param device_dir True if it should open the file in the device_directory, false it will search the port directory for the file 142 | * @param openFile Storage for file pointer 143 | * @return True if read successful, false otherwise 144 | */ 145 | bool writeInt(const char *filename, int value, OpenFile& openFile, bool device_dir=true); 146 | 147 | 148 | /** 149 | * @return The name of the port 150 | */ 151 | const std::string& getPortName() const 152 | { 153 | return port_name_; 154 | } 155 | 156 | 157 | /** 158 | * @return The ports type 159 | */ 160 | H4Ev3IoPortType getPortType() const 161 | { 162 | return port_type_; 163 | } 164 | 165 | /** 166 | * get driver name from current device 167 | * @param[out] drvname Driver name enum type 168 | * @return True if successful, false otherwise (false normally means, nothing is connected) 169 | */ 170 | bool getDriverName(Ev3Strings::Ev3DriverName &drvname) 171 | { 172 | if(!isConnected()) 173 | { 174 | return false; 175 | } 176 | 177 | 178 | //Reduce file reads and string processing ... 179 | //Was the last connection id the same as on last check? 180 | if(connect_id_==last_driver_check_connected_id_) 181 | { 182 | //If yes we can use the stored value 183 | drvname=last_driver_; 184 | return true; 185 | } 186 | else //if not we read the current driver name and convert it to enum 187 | { 188 | bool ret=readKey("driver_name",Ev3Strings::ev3_driver_name_conv,drvname,f_driver_name); 189 | std::cout<<"Driver Name: "< 214 | bool writeKey(const char *filename, const StringEnum< T > &strmap, T key, OpenFile& openFile, bool device_dir=true) 215 | { 216 | 217 | FILE *file=get_fileptr_(filename, OpenFile::MODE_W, openFile, device_dir); 218 | 219 | if(file==NULL) 220 | return false; 221 | 222 | return writeKeyToSysFile(file,strmap,key); 223 | } 224 | 225 | 226 | /** 227 | * Reads a /sys string file and compares the read string with a given map of strings and gets it's number from the map 228 | * @param filename The sys file to be read 229 | * @param strmap The string map 230 | * @param key The key of the string (if return TRUE!) 231 | * @param device_dir True if it should search in the device_directory, false it will search the port directory for the file 232 | * @param openFile Storage for file pointer 233 | * @return True if read successful, false otherwise 234 | */ 235 | template 236 | bool readKey(const char *filename, const StringEnum< T > &strmap, T &key, OpenFile& openFile, bool device_dir=true) 237 | { 238 | 239 | FILE *file=get_fileptr_(filename, OpenFile::MODE_R, openFile, device_dir); 240 | 241 | if(file==NULL) 242 | return false; 243 | 244 | return readKeyFromSysFile(file,strmap,key); 245 | } 246 | }; 247 | 248 | 249 | class H4REv3Motor : public H4REv3Port 250 | { 251 | 252 | OpenFile f_DutyCycleSP; 253 | OpenFile f_SpeedRegulation; 254 | OpenFile f_SpeedSP; 255 | OpenFile f_PositionSP; 256 | OpenFile f_Speed; 257 | OpenFile f_Position; 258 | OpenFile f_MotorCommand; 259 | OpenFile f_MotorPolarity; 260 | OpenFile f_SpeedPID_Kp; 261 | OpenFile f_SpeedPID_Ki; 262 | OpenFile f_SpeedPID_Kd; 263 | 264 | public: 265 | 266 | /** 267 | * 268 | * @param port_name 269 | */ 270 | H4REv3Motor(const std::string &port_name) 271 | :H4REv3Port(port_name,H4REV3PORT_OUT) 272 | {} 273 | 274 | /** 275 | * Sets the pwm duty cycle value 276 | * @param value The pwm value 277 | * @return True if successful, false otherwise 278 | */ 279 | bool setDutyCycleSP(int value) 280 | { 281 | return writeInt("duty_cycle_sp", value, f_DutyCycleSP); 282 | } 283 | 284 | /** 285 | * This function enables or disables the speed regulation functionality 286 | * @param onoff On/Off 287 | * @return True if successful, false otherwise 288 | */ 289 | bool setSpeedRegulation(Ev3Strings::Ev3Switch onoff) 290 | { 291 | return writeKey("speed_regulation", Ev3Strings::ev3_switch_conv, onoff, f_SpeedRegulation); 292 | } 293 | 294 | /** 295 | * Set speed setpoint 296 | * @param value The speed value in 1/10 rounds per second 297 | * @return True if successful, false otherwise 298 | */ 299 | bool setSpeedSP(int value) 300 | { 301 | return writeInt("speed_sp",value,f_SpeedSP); 302 | } 303 | 304 | /** 305 | * Set Position setpoint for position control 306 | * @param value The position value 307 | * @return True if successful, false otherwise 308 | */ 309 | bool setPositionSP(int value) 310 | { 311 | return writeInt("position_sp",value,f_PositionSP); 312 | } 313 | 314 | /** 315 | * Get the current position of the motor 316 | * @param[out] value The position value if return is true 317 | * @return True if successful, false otherwise 318 | */ 319 | bool position(int &value) 320 | { 321 | return readInt("position",value, f_Position); 322 | } 323 | 324 | /** 325 | * Get the current speed of the motor 326 | * @param[out] value The position value if return is true 327 | * @return True if successful, false otherwise 328 | */ 329 | bool speed(int &value) 330 | { 331 | return readInt("speed",value, f_Speed); 332 | } 333 | 334 | /** 335 | * Setting the command (especially for motors) 336 | * @param command The 337 | * @return True if successful, false otherwise 338 | */ 339 | bool setMotorCommand(Ev3Strings::Ev3MotorCommands command) 340 | { 341 | return writeKey("command",Ev3Strings::ev3_motor_commands_conv, command, f_MotorCommand); 342 | } 343 | 344 | /** 345 | * Sets the rotation direction of the motor 346 | * @param pol The direction 347 | * @return True if successful 348 | */ 349 | bool setMotorPolarity(Ev3Strings::Ev3Polarity pol) 350 | { 351 | return writeKey("polarity",Ev3Strings::ev3_polarity_conv, pol, f_MotorPolarity); 352 | } 353 | 354 | /** 355 | * Sets the Kp part of the motors speed control 356 | * @param value 357 | * @return True if successful 358 | */ 359 | bool setSpeedPID_Kp(unsigned int value) 360 | { 361 | return writeInt("speed_pid/Kp",value,f_SpeedPID_Kp); 362 | } 363 | 364 | /** 365 | * Sets the Ki part of the motors speed control 366 | * @param value 367 | * @return True if successful 368 | */ 369 | bool setSpeedPID_Ki(unsigned int value) 370 | { 371 | return writeInt("speed_pid/Ki",value,f_SpeedPID_Ki); 372 | } 373 | 374 | /** 375 | * Sets the Kd part of the motors speed control 376 | * @param value 377 | * @return True if successful 378 | */ 379 | bool setSpeedPID_Kd(unsigned int value) 380 | { 381 | return writeInt("speed_pid/Kd",value,f_SpeedPID_Kd); 382 | } 383 | }; 384 | 385 | 386 | class H4REv3Sensor : public H4REv3Port 387 | { 388 | public: 389 | H4REv3Sensor(const std::string &port_name) 390 | :H4REv3Port(port_name,H4REV3PORT_IN) 391 | {} 392 | 393 | private: 394 | OpenFile f_mode; 395 | OpenFile f_NumValues; 396 | OpenFile f_Value[8]; 397 | 398 | public: 399 | unsigned num_values() 400 | { 401 | int val=0; 402 | readInt("num_values",val,f_NumValues); 403 | return val; 404 | } 405 | 406 | bool value(unsigned number, int &value) 407 | { 408 | 409 | char numstring[]="value0"; 410 | 411 | numstring[5]+=number; 412 | 413 | if(number 425 | bool setModeT(const StringEnum< T > &modemap, T key) 426 | { 427 | 428 | FILE *file=get_fileptr_("mode", OpenFile::MODE_W, f_mode); 429 | 430 | if(file==NULL) 431 | return false; 432 | 433 | return writeKeyToSysFile(file,modemap,key); 434 | } 435 | }; 436 | 437 | 438 | template & MODE_CONV, Ev3Strings::Ev3DriverName DRV> 439 | class H4REv3SensorSpecific 440 | { 441 | public: 442 | 443 | H4REv3Sensor *sensor_; 444 | H4REv3SensorSpecific() 445 | :sensor_(0) 446 | {} 447 | 448 | H4REv3SensorSpecific(H4REv3Sensor *sensor) 449 | :sensor_(sensor) 450 | {} 451 | 452 | void setSensor(H4REv3Sensor *sensor) 453 | { 454 | sensor_=sensor; 455 | } 456 | 457 | H4REv3Sensor* getSensor() 458 | { 459 | return sensor_; 460 | } 461 | 462 | 463 | bool isConnected() 464 | { 465 | if(!sensor_) 466 | return false; 467 | 468 | if(sensor_->isConnected()) 469 | { 470 | Ev3Strings::Ev3DriverName drvname; 471 | if(sensor_->getDriverName(drvname)) 472 | { 473 | return drvname==DRV; 474 | } 475 | else 476 | { 477 | return false; 478 | } 479 | } 480 | else 481 | { 482 | return false; 483 | } 484 | } 485 | 486 | bool setMode(MODE_ENUM mode) 487 | { 488 | if(!sensor_) 489 | return false; 490 | return sensor_->setModeT(MODE_CONV, mode); 491 | } 492 | }; 493 | 494 | typedef H4REv3SensorSpecific H4REv3UltraSonicSensorSpecIface; 495 | typedef H4REv3SensorSpecific H4REv3GyroSensorSpecIface; 496 | typedef H4REv3SensorSpecific H4REv3TouchSensorSpecIface; 497 | typedef H4REv3SensorSpecific H4REv3ColorSensorSpecIface; 498 | typedef H4REv3SensorSpecific H4REv3IRSensorSpecIface; 499 | 500 | 501 | }/*ev3_control*/ 502 | 503 | 504 | #endif /* H4REV3PORT_H_ */ 505 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/StringEnum.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (StringEnum.h) is part of h4r_ev3_control. 3 | * Date: 18.12.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef STRINGENUM_H_ 24 | #define STRINGENUM_H_ 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace ev3_control 36 | { 37 | 38 | using namespace std; 39 | template 40 | class StringEnum 41 | { 42 | 43 | class Entry 44 | { 45 | 46 | public: 47 | Entry(unsigned key, const char* str) 48 | :str_(str) 49 | ,key_(key) 50 | {} 51 | 52 | bool operator<(const Entry& e) const 53 | { 54 | return str_ key_from_string_; 81 | std::vector string_from_key_; 82 | 83 | public: 84 | 85 | StringEnum() 86 | :string_count_(0) 87 | ,final_(false) 88 | {} 89 | 90 | void insert(const char *str) 91 | { 92 | assert(final_==false && "already finalized!"); 93 | 94 | key_from_string_.push_back(Entry(string_count_,str)); 95 | string_from_key_.push_back(string(str)); 96 | string_count_++; 97 | } 98 | 99 | void finalize() 100 | { 101 | std::sort(key_from_string_.begin(), key_from_string_.end()); 102 | final_=true; 103 | } 104 | 105 | const char* operator[](ENUM key) const 106 | { 107 | assert(key < string_count_ && "Key out of range!"); 108 | return string_from_key_[key].c_str(); 109 | } 110 | 111 | ENUM operator[](const char* str) const 112 | { 113 | assert(final_ && "StringEnum not finalized!"); 114 | 115 | cout<<"str:"<. 20 | * 21 | */ 22 | 23 | 24 | #ifndef EV3_COLOR_CONTROLLER_H_ 25 | #define EV3_COLOR_CONTROLLER_H_ 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | 38 | namespace ev3_control 39 | { 40 | 41 | /** 42 | * \page Ev3ColorController Ev3ColorController 43 | * \ingroup Ev3Controllers 44 | * \image html EV3Color.png 45 | * \section Controller Function 46 | * This controller is used to read the standard EV3 Color Sensor 47 | * 48 | * \example color.launch 49 | * \example color.yaml 50 | */ 51 | 52 | /** 53 | * \ingroup Ev3Controllers 54 | */ 55 | class Ev3ColorController: public controller_interface::Controller< 56 | Ev3SensorInterface> 57 | { 58 | private: 59 | std::string port_; 60 | Ev3Strings::Ev3ColorMode mode_; 61 | Ev3SensorHandle handle_; 62 | H4REv3ColorSensorSpecIface color_interface_; 63 | bool sensor_mode_needs_init_; 64 | std::string frame_id_; 65 | 66 | //Range Publisher 67 | typedef boost::shared_ptr< 68 | realtime_tools::RealtimePublisher > RtIlluminancePublisherPtr; 69 | RtIlluminancePublisherPtr realtime_illuminance_publisher_; 70 | 71 | typedef boost::shared_ptr< 72 | realtime_tools::RealtimePublisher > RtColorPublisherPtr; 73 | RtColorPublisherPtr realtime_color_publisher_; 74 | 75 | typedef boost::shared_ptr< 76 | realtime_tools::RealtimePublisher > RtColorNumberPublisherPtr; 77 | RtColorNumberPublisherPtr realtime_color_number_publisher_; 78 | 79 | ros::Time last_publish_time_; 80 | double publish_rate_; 81 | 82 | public: 83 | Ev3ColorController(); 84 | virtual ~Ev3ColorController(); 85 | 86 | virtual bool init(Ev3SensorInterface* hw, 87 | ros::NodeHandle &root_nh, 88 | ros::NodeHandle& ctrl_nh); 89 | 90 | virtual void starting(const ros::Time& time); 91 | virtual void update(const ros::Time& time, const ros::Duration& /*period*/); 92 | virtual void stopping(const ros::Time& /*time*/){} 93 | 94 | }; 95 | 96 | } /* namespace ev3_control */ 97 | 98 | #endif /* EV3_COLOR_CONTROLLER_H_ */ 99 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/ev3_gyro_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_gyro_controller.h) is part of h4r_ev3_control. 3 | * Date: 01.01.2016 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | #ifndef EV3_GYRO_CONTROLLER_H_ 23 | #define EV3_GYRO_CONTROLLER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace ev3_control 36 | { 37 | /** 38 | * \page Ev3GyroController Ev3GyroController 39 | * \image html EV3Gyro.png 40 | * \ingroup Ev3Controllers 41 | * \section Controller Function 42 | * This controller is used to read the standard EV3 Gyro Sensor 43 | * 44 | * \example gyro.launch 45 | * \example gyro.yaml 46 | */ 47 | 48 | /** 49 | * \ingroup Ev3Controllers 50 | */ 51 | class Ev3GyroController: public controller_interface::Controller< 52 | Ev3SensorInterface> 53 | { 54 | private: 55 | std::string port_; 56 | Ev3Strings::Ev3GyroMode mode_; 57 | Ev3SensorHandle handle_; 58 | H4REv3GyroSensorSpecIface gyro_interface_; 59 | bool sensor_mode_needs_init_; 60 | 61 | std::string frame_id_; 62 | 63 | //Range Publisher 64 | typedef boost::shared_ptr< 65 | realtime_tools::RealtimePublisher > RtImuPublisherPtr; 66 | RtImuPublisherPtr realtime_imu_publisher_; 67 | 68 | 69 | 70 | ros::Time last_publish_time_; 71 | double publish_rate_; 72 | 73 | public: 74 | Ev3GyroController(); 75 | virtual ~Ev3GyroController(); 76 | 77 | virtual bool init(Ev3SensorInterface* hw, 78 | ros::NodeHandle &root_nh, 79 | ros::NodeHandle& ctrl_nh); 80 | 81 | virtual void starting(const ros::Time& time); 82 | virtual void update(const ros::Time& time, const ros::Duration& /*period*/); 83 | virtual void stopping(const ros::Time& /*time*/){} 84 | 85 | }; 86 | 87 | } /* namespace ev3_control */ 88 | 89 | #endif /* EV3_GYRO_CONTROLLER_H_ */ 90 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/ev3_infrared_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_infrared_controller.h) is part of h4r_ev3_control. 3 | * Date: 06.01.2016 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | #ifndef EV3_INFRARED_CONTROLLER_H_ 23 | #define EV3_INFRARED_CONTROLLER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | 34 | #include 35 | 36 | namespace ev3_control 37 | { 38 | 39 | /** 40 | * \page Ev3InfraredController Ev3InfraredController 41 | * \ingroup Ev3Controllers 42 | * \image html EV3Infrared.png 43 | * \section Controller Function 44 | * This controller is used to read the standard EV3 Infrared Sensor 45 | * 46 | * \example infrared.launch 47 | * \example infrared.yaml 48 | */ 49 | 50 | /** 51 | * \ingroup Ev3Controllers 52 | */ 53 | class Ev3InfraredController: public controller_interface::Controller< 54 | Ev3SensorInterface> 55 | { 56 | private: 57 | std::string port_; 58 | Ev3Strings::Ev3InfraredMode mode_; 59 | Ev3SensorHandle handle_; 60 | H4REv3IRSensorSpecIface ir_interface_; 61 | bool sensor_mode_needs_init_; 62 | 63 | double max_range_; 64 | double min_range_; 65 | std::string frame_id_; 66 | unsigned value_number_; 67 | 68 | //Range Publisher 69 | typedef boost::shared_ptr< 70 | realtime_tools::RealtimePublisher > RtRangePublisherPtr; 71 | RtRangePublisherPtr realtime_range_publisher_; 72 | 73 | typedef boost::shared_ptr< 74 | realtime_tools::RealtimePublisher > RtSeekPublisherPtr; 75 | RtSeekPublisherPtr realtime_seek_publishers_[4]; 76 | 77 | typedef boost::shared_ptr< 78 | realtime_tools::RealtimePublisher > RtJoyPublisherPtr; 79 | RtJoyPublisherPtr realtime_joy_publishers_[4]; 80 | 81 | 82 | ros::Time last_publish_time_[4]; 83 | double publish_rate_; 84 | bool first_time_[4]; 85 | 86 | public: 87 | Ev3InfraredController(); 88 | virtual ~Ev3InfraredController(); 89 | 90 | virtual bool init(Ev3SensorInterface* hw, 91 | ros::NodeHandle &root_nh, 92 | ros::NodeHandle& ctrl_nh); 93 | 94 | virtual void starting(const ros::Time& time); 95 | virtual void update(const ros::Time& time, const ros::Duration& /*period*/); 96 | virtual void stopping(const ros::Time& /*time*/){} 97 | 98 | }; 99 | 100 | } /* namespace ev3_control */ 101 | 102 | #endif /* EV3_INFRARED_CONTROLLER_H_ */ 103 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/ev3_touch_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_touch_controller.h) is part of h4r_ev3_control. 3 | * Date: 01.01.2016 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | #ifndef TOUCH_CONTROLLER_H_ 23 | #define TOUCH_CONTROLLER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | #include 33 | 34 | namespace ev3_control 35 | { 36 | 37 | /** 38 | * \page Ev3TouchController Ev3TouchController 39 | * \ingroup Ev3Controllers 40 | * \image html EV3Touch.png 41 | * \section Controller Function 42 | * This controller is used to read the standard EV3 Touch Sensor 43 | * 44 | * \example touch.launch 45 | * \example touch.yaml 46 | */ 47 | 48 | /** 49 | * \ingroup Ev3Controllers 50 | */ 51 | class Ev3TouchController: public controller_interface::Controller< 52 | Ev3SensorInterface> 53 | { 54 | private: 55 | std::string port_; 56 | Ev3SensorHandle handle_; 57 | H4REv3TouchSensorSpecIface touch_interface_; 58 | bool sensor_mode_needs_init_; 59 | 60 | std::string frame_id_; 61 | 62 | //Range Publisher 63 | typedef boost::shared_ptr< 64 | realtime_tools::RealtimePublisher > RtBoolPublisherPtr; 65 | RtBoolPublisherPtr realtime_bool_publisher_; 66 | 67 | 68 | 69 | ros::Time last_publish_time_; 70 | double publish_rate_; 71 | 72 | public: 73 | Ev3TouchController(); 74 | virtual ~Ev3TouchController(); 75 | 76 | virtual bool init(Ev3SensorInterface* hw, 77 | ros::NodeHandle &root_nh, 78 | ros::NodeHandle& ctrl_nh); 79 | 80 | virtual void starting(const ros::Time& time); 81 | virtual void update(const ros::Time& time, const ros::Duration& /*period*/); 82 | virtual void stopping(const ros::Time& /*time*/){} 83 | 84 | }; 85 | 86 | } /* namespace ev3_control */ 87 | 88 | #endif /* TOUCH_CONTROLLER_H_ */ 89 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/ev3_ultrasonic_controller.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (Ev3UltraSonicController.h) is part of h4r_ev3_control. 3 | * Date: 10.12.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | #ifndef EV3_ULTRASONIC_CONTROLLER_H_ 23 | #define EV3_ULTRASONIC_CONTROLLER_H_ 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | 35 | namespace ev3_control 36 | { 37 | 38 | /** 39 | * \page Ev3UltrasonicController Ev3UltrasonicController 40 | * \ingroup Ev3Controllers 41 | * \image html EV3Ultrasonic.png 42 | * \section Controller Function 43 | * This controller is used to read the standard EV3 Ultrasonic Sensor 44 | * 45 | * \example ultrasonic.launch 46 | * \example ultrasonic.yaml 47 | */ 48 | 49 | /** 50 | * \ingroup Ev3Controllers 51 | */ 52 | class Ev3UltrasonicController: public controller_interface::Controller< 53 | Ev3SensorInterface> 54 | { 55 | private: 56 | std::string port_; 57 | Ev3Strings::Ev3UltrasonicMode mode_; 58 | Ev3SensorHandle handle_; 59 | H4REv3UltraSonicSensorSpecIface us_interface_; 60 | bool sensor_mode_needs_init_; 61 | 62 | double max_range_; 63 | double min_range_; 64 | std::string frame_id_; 65 | 66 | //Range Publisher 67 | typedef boost::shared_ptr< 68 | realtime_tools::RealtimePublisher > RtRangePublisherPtr; 69 | RtRangePublisherPtr realtime_range_publisher_; 70 | 71 | typedef boost::shared_ptr< 72 | realtime_tools::RealtimePublisher > RtBoolPublisherPtr; 73 | RtBoolPublisherPtr realtime_bool_publisher_; 74 | 75 | 76 | ros::Time last_publish_time_; 77 | double publish_rate_; 78 | 79 | public: 80 | Ev3UltrasonicController(); 81 | virtual ~Ev3UltrasonicController(); 82 | 83 | virtual bool init(Ev3SensorInterface* hw, 84 | ros::NodeHandle &root_nh, 85 | ros::NodeHandle& ctrl_nh); 86 | 87 | virtual void starting(const ros::Time& time); 88 | virtual void update(const ros::Time& time, const ros::Duration& /*period*/); 89 | virtual void stopping(const ros::Time& /*time*/){} 90 | 91 | }; 92 | 93 | } /* namespace ev3_control */ 94 | 95 | #endif /* EV3_ULTRASONIC_CONTROLLER_H_ */ 96 | -------------------------------------------------------------------------------- /h4r_ev3_control/include/h4r_ev3_control/syshelpers.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (syshelpers.h) is part of h4r_ev3_control. 3 | * Date: 22.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | #ifndef SYSHELPERS_H_ 23 | #define SYSHELPERS_H_ 24 | 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | #include 34 | #include 35 | 36 | namespace ev3_control 37 | { 38 | 39 | /** 40 | * A file name buffer without memory allocation 41 | */ 42 | typedef FixedBuffer::FixedBuffer<256> FileNameBuffer; 43 | 44 | /** 45 | * Check if file exists 46 | * @param path Path to be checked of access 47 | * @return True if it exists 48 | */ 49 | bool pathExists(const char* path); 50 | 51 | 52 | /** 53 | * Write int value into a opened (sys) file. 54 | * @param fileptr The file pointer from fopen 55 | * @param value The value 56 | * @return true if everything was successful, false otherwise 57 | */ 58 | bool writeIntToSysFile(FILE *fileptr,int value); 59 | 60 | /** 61 | * Read integer value from (sys) file 62 | * @param fileptr The file pointer from fopen 63 | * @param[out] value The value in the file only valid if successful 64 | * @return true if everything was successful, false otherwise 65 | */ 66 | bool readIntFromSysFile(FILE *fileptr, int &value); 67 | 68 | /** 69 | * Write a String in a map to a (sys) file 70 | * @param fileptr The file pointer 71 | * @param strmap The map with the strings and the keys 72 | * @param key The key (enum value) 73 | * @return True if everything was successful, false otherwise 74 | */ 75 | template 76 | bool writeKeyToSysFile(FILE *fileptr, const StringEnum &strmap, T key) 77 | { 78 | 79 | char outbuf[256]; 80 | int len=strlen(strmap[key]); 81 | strncpy(outbuf, strmap[key], len); 82 | 83 | int wrote=fwrite(outbuf,1,len,fileptr); 84 | 85 | if(fflush(fileptr)) 86 | return false; 87 | rewind(fileptr); 88 | 89 | return wrote==len; 90 | } 91 | 92 | /** 93 | * Reads a sysfile and checks if the string exists in a map and returns the value 94 | * @param fileptr 95 | * @param strmap 96 | * @param value The value for the string 97 | * @return True if everything was ok, false otherwise 98 | */ 99 | template 100 | bool readKeyFromSysFile(FILE *fileptr,const StringEnum &strmap, T &value) 101 | { 102 | fflush(fileptr); 103 | rewind(fileptr); 104 | int64_t value_out; 105 | 106 | char buffer[256]; 107 | char *buf=&buffer[0]; 108 | char **bufptr=&buf; 109 | 110 | bool negative; 111 | ssize_t read; 112 | size_t len=256; 113 | int l=0; 114 | bool ok=false; 115 | while ((read = getline(bufptr, &len, fileptr)) != -1) { 116 | if(l==0) 117 | { 118 | buffer[read-1]=0x00;//remove linefeed! 119 | value=strmap[buffer]; 120 | if(value<0) 121 | { 122 | return false; 123 | } 124 | else 125 | { 126 | ok=true; 127 | } 128 | 129 | } 130 | else 131 | { 132 | if(l!=1 || read!=1) 133 | { 134 | return false; 135 | } 136 | } 137 | 138 | return ok; 139 | } 140 | 141 | 142 | fscanf(fileptr,"%[^\n]",buffer); 143 | return true; 144 | } 145 | 146 | /** 147 | * Searches for the same file with the same one line string content in multiple subdirectories 148 | * Used for identifying a device by name file in sys directories 149 | * @param parent The class directory to search 150 | * @param file The file to search in the directories 151 | * @param content The content which should match 152 | * @param[out] match_dir The first matching directory 153 | * @return True if found, false otherwise 154 | */ 155 | bool matchFileContentInEqualSubdirectories(const char* parent, 156 | const char* file, 157 | const char* content, 158 | FileNameBuffer &match_dir); 159 | 160 | } /*ev3_control*/ 161 | #endif /* SYSHELPERS_H_ */ 162 | -------------------------------------------------------------------------------- /h4r_ev3_control/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | h4r_ev3_control 4 | 0.0.0 5 | The h4r_ev3_control package 6 | 7 | Christian Holl 8 | 9 | GPLv3 10 | 11 | http://hacks4ros.github.io/h4r_ev3_ctrl 12 | http://github.com/Hacks4ROS/h4r_ev3_ctrl 13 | https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues 14 | 15 | Christian Holl 16 | 17 | catkin 18 | controller_interface 19 | hardware_interface 20 | pluginlib 21 | realtime_tools 22 | roscpp 23 | sensor_msgs 24 | h4r_ev3_msgs 25 | 26 | controller_interface 27 | hardware_interface 28 | pluginlib 29 | realtime_tools 30 | roscpp 31 | sensor_msgs 32 | h4r_ev3_msgs 33 | 34 | 35 | 36 | 37 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/Ev3Strings.cpp: -------------------------------------------------------------------------------- 1 | 2 | /*WARNING WARNING WARNING WARNING WARNING 3 | *This file is generated by script 4 | *generateMaps.py, to add strings change 5 | *strings.yml and run it again! 6 | *WARNING WARNING WARNING WARNING WARNING 7 | */ 8 | #include 9 | namespace ev3_control{ 10 | StringEnum init_ev3_switch_conv() 11 | { 12 | StringEnum mp; 13 | mp.insert("on"); 14 | mp.insert("off"); 15 | mp.finalize(); 16 | return mp; 17 | } 18 | 19 | StringEnumEv3Strings::ev3_switch_conv = init_ev3_switch_conv(); 20 | 21 | 22 | 23 | StringEnum init_ev3_ultrasonic_mode_conv() 24 | { 25 | StringEnum mp; 26 | mp.insert("US-DC-IN"); 27 | mp.insert("US-DIST-CM"); 28 | mp.insert("US-LISTEN"); 29 | mp.insert("US-DC-CM"); 30 | mp.insert("US-SI-IN"); 31 | mp.insert("US-SI-CM"); 32 | mp.insert("US-DIST-IN"); 33 | mp.finalize(); 34 | return mp; 35 | } 36 | 37 | StringEnumEv3Strings::ev3_ultrasonic_mode_conv = init_ev3_ultrasonic_mode_conv(); 38 | 39 | 40 | 41 | StringEnum init_ev3_color_mode_conv() 42 | { 43 | StringEnum mp; 44 | mp.insert("COL-COLOR"); 45 | mp.insert("COL-AMBIENT"); 46 | mp.insert("COL-REFLECT"); 47 | mp.insert("RGB-RAW"); 48 | mp.insert("COL-CAL"); 49 | mp.insert("REF-RAW"); 50 | mp.finalize(); 51 | return mp; 52 | } 53 | 54 | StringEnumEv3Strings::ev3_color_mode_conv = init_ev3_color_mode_conv(); 55 | 56 | 57 | 58 | StringEnum init_ev3_infrared_mode_conv() 59 | { 60 | StringEnum mp; 61 | mp.insert("IR-REMOTE"); 62 | mp.insert("IR-PROX"); 63 | mp.insert("IR-REM-A"); 64 | mp.insert("IR-SEEK"); 65 | mp.insert("IR-S-ALT"); 66 | mp.insert("IR-CAL"); 67 | mp.finalize(); 68 | return mp; 69 | } 70 | 71 | StringEnumEv3Strings::ev3_infrared_mode_conv = init_ev3_infrared_mode_conv(); 72 | 73 | 74 | 75 | StringEnum init_ev3_driver_name_conv() 76 | { 77 | StringEnum mp; 78 | mp.insert("lego-ev3-touch"); 79 | mp.insert("lego-ev3-color"); 80 | mp.insert("lego-ev3-us"); 81 | mp.insert("lego-ev3-gyro"); 82 | mp.insert("lego-ev3-ir"); 83 | mp.finalize(); 84 | return mp; 85 | } 86 | 87 | StringEnumEv3Strings::ev3_driver_name_conv = init_ev3_driver_name_conv(); 88 | 89 | 90 | 91 | StringEnum init_ev3_polarity_conv() 92 | { 93 | StringEnum mp; 94 | mp.insert("inversed"); 95 | mp.insert("normal"); 96 | mp.finalize(); 97 | return mp; 98 | } 99 | 100 | StringEnumEv3Strings::ev3_polarity_conv = init_ev3_polarity_conv(); 101 | 102 | 103 | 104 | StringEnum init_ev3_port_status_conv() 105 | { 106 | StringEnum mp; 107 | mp.insert("ev3-uart"); 108 | mp.insert("no-connect"); 109 | mp.insert("ev3-analog"); 110 | mp.finalize(); 111 | return mp; 112 | } 113 | 114 | StringEnumEv3Strings::ev3_port_status_conv = init_ev3_port_status_conv(); 115 | 116 | 117 | 118 | StringEnum init_ev3_gyro_mode_conv() 119 | { 120 | StringEnum mp; 121 | mp.insert("GYRO-FAS"); 122 | mp.insert("GYRO-ANG"); 123 | mp.insert("GYRO-G&A"); 124 | mp.insert("GYRO-CAL"); 125 | mp.insert("GYRO-RATE"); 126 | mp.finalize(); 127 | return mp; 128 | } 129 | 130 | StringEnumEv3Strings::ev3_gyro_mode_conv = init_ev3_gyro_mode_conv(); 131 | 132 | 133 | 134 | StringEnum init_ev3_motor_commands_conv() 135 | { 136 | StringEnum mp; 137 | mp.insert("reset"); 138 | mp.insert("run-to-rel-pos"); 139 | mp.insert("run-forever"); 140 | mp.insert("run-direct"); 141 | mp.insert("stop"); 142 | mp.insert("run-timed"); 143 | mp.insert("run-to-abs-pos"); 144 | mp.finalize(); 145 | return mp; 146 | } 147 | 148 | StringEnumEv3Strings::ev3_motor_commands_conv = init_ev3_motor_commands_conv(); 149 | 150 | 151 | 152 | StringEnum init_ev3_touch_mode_conv() 153 | { 154 | StringEnum mp; 155 | mp.insert("TOUCH"); 156 | mp.finalize(); 157 | return mp; 158 | } 159 | 160 | StringEnumEv3Strings::ev3_touch_mode_conv = init_ev3_touch_mode_conv(); 161 | 162 | 163 | 164 | StringEnum init_ev3_port_drivers_conv() 165 | { 166 | StringEnum mp; 167 | mp.insert("legoev3-input-port"); 168 | mp.insert("legoev3-input-outport"); 169 | mp.finalize(); 170 | return mp; 171 | } 172 | 173 | StringEnumEv3Strings::ev3_port_drivers_conv = init_ev3_port_drivers_conv(); 174 | 175 | 176 | 177 | } 178 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/H4REv3Port.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (H4REv3Port.cpp) is part of h4r_ev3_control. 3 | * Date: 22.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | #include 23 | 24 | 25 | namespace ev3_control 26 | { 27 | 28 | H4REv3Port::H4REv3Port(const std::string &port_name, H4Ev3IoPortType port_type) 29 | :port_name_(port_name) 30 | ,port_type_(port_type) 31 | ,connected_(false) 32 | ,last_driver_check_connected_id_(0) 33 | ,last_driver_(Ev3Strings::EV3DRIVERNAME_NOT_FOUND) 34 | { 35 | if(!getPortDirectory()) 36 | { 37 | //TODO throw error 38 | } 39 | connected_=getDeviceDirectory(); 40 | 41 | } 42 | 43 | 44 | H4REv3Port::~H4REv3Port() 45 | { 46 | //TODO Close files 47 | } 48 | 49 | bool H4REv3Port::isConnected() 50 | { 51 | if(connected_) 52 | { 53 | connected_ = pathExists(sys_device_directory_.c_str()); 54 | } 55 | else 56 | { 57 | getDeviceDirectory(); 58 | } 59 | 60 | return connected_; 61 | } 62 | 63 | 64 | 65 | bool H4REv3Port::getPortDirectory() 66 | { 67 | return matchFileContentInEqualSubdirectories("/sys/class/lego-port","port_name",port_name_.c_str(),sys_port_directory_); 68 | } 69 | 70 | bool H4REv3Port::getDeviceDirectory() 71 | { 72 | 73 | connected_=false; 74 | switch(port_type_) 75 | { 76 | case H4REV3PORT_IN: 77 | if(matchFileContentInEqualSubdirectories("/sys/class/lego-sensor","port_name",port_name_.c_str(), sys_device_directory_)) 78 | { 79 | connect_id_++; 80 | connected_=true; 81 | } 82 | break; 83 | 84 | 85 | case H4REV3PORT_OUT: 86 | if(matchFileContentInEqualSubdirectories("/sys/class/tacho-motor","port_name",port_name_.c_str(), sys_device_directory_)) 87 | { 88 | connect_id_++; 89 | connected_=true; 90 | } 91 | break; 92 | 93 | default: 94 | break; 95 | } 96 | 97 | return connected_; 98 | } 99 | 100 | 101 | FILE* H4REv3Port::get_fileptr_(const char* filename, OpenFile::FileMode mode, OpenFile &file, bool device_dir) 102 | { 103 | 104 | if(connected_==false) 105 | { 106 | if(!getDeviceDirectory()) 107 | { 108 | return NULL; 109 | } 110 | } 111 | else 112 | { 113 | if(!isConnected()) 114 | return NULL; 115 | } 116 | 117 | if(file.ptr!=NULL 118 | && connect_id_ == file.connect_id_ ) 119 | { 120 | return file.ptr; 121 | } 122 | else 123 | { 124 | file.ptr=NULL; 125 | file.connect_id_=connect_id_; 126 | 127 | char const *smode; 128 | switch(mode) 129 | { 130 | case OpenFile::MODE_R: 131 | smode="r"; 132 | break; 133 | case OpenFile::MODE_W: 134 | smode="w"; 135 | break; 136 | case OpenFile::MODE_RW: 137 | smode="rw"; 138 | } 139 | 140 | char const *dir; 141 | if(device_dir) 142 | { 143 | dir=sys_device_directory_.c_str(); 144 | } 145 | else 146 | { 147 | dir=sys_port_directory_.c_str(); 148 | } 149 | 150 | file.fullpath.format("%s/%s",dir,filename); 151 | file.ptr=fopen(file.fullpath.c_str(),smode); 152 | 153 | if(file.ptr==0) 154 | file.fullpath=""; 155 | 156 | std::cout<<"getFilePTR opening: "<. 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace ev3_control 26 | { 27 | 28 | Ev3ColorController::Ev3ColorController() 29 | :sensor_mode_needs_init_(true) 30 | ,mode_(Ev3Strings::EV3COLORMODE_RGB_RAW) 31 | ,publish_rate_(10) 32 | { 33 | // TODO Auto-generated constructor stub 34 | 35 | } 36 | 37 | Ev3ColorController::~Ev3ColorController() 38 | { 39 | // TODO Auto-generated destructor stub 40 | } 41 | 42 | 43 | 44 | bool Ev3ColorController::init(Ev3SensorInterface* hw, 45 | ros::NodeHandle &root_nh, 46 | ros::NodeHandle& ctrl_nh) 47 | { 48 | 49 | // get publishing period 50 | /** 51 | * 52 | * \page Ev3ColorController Ev3ColorController 53 | * \section Parameters 54 | * \subsection publish_rate 55 | * 56 | * The rate which the controller should use to publish its messages. 57 | */ 58 | if (!ctrl_nh.getParam("publish_rate", publish_rate_)) 59 | { 60 | ROS_ERROR("Parameter publish_rate was not set, using 10 Hz"); 61 | } 62 | 63 | 64 | /** 65 | * \page Ev3ColorController Ev3ColorController 66 | * \subsection port 67 | * 68 | * 69 | * Specifies the EV3 port name. 70 | */ 71 | if (!ctrl_nh.getParam("port", port_)) 72 | { 73 | ROS_ERROR("Parameter port was not set"); 74 | return false; 75 | } 76 | 77 | std::string mode_str; 78 | 79 | 80 | /** 81 | * \page Ev3ColorController Ev3ColorController 82 | * \subsection mode 83 | * 84 | * The mode parameter sets the mode for the color sensor. If not set, it will use **rgb_raw**. 85 | * 86 | * The following modes are possible for the sensor: 87 | * 1. **rgb_raw**\n 88 | * 'rgb_raw' mode uses the RGB_RAW mode of this sensor. 89 | * It will output the color information in RGB in a **std_msgs::ColorRGBA.msg** topic. 90 | * 2. **color**\n 91 | * 'color' mode publishes the a number for the following recognized colors into a **std_msgs::UInt8** topic\n 92 | * Value | Color 93 | * ------|------ 94 | * 0 | none 95 | * 1 | black 96 | * 2 | blue 97 | * 3 | green 98 | * 4 | yellow 99 | * 5 | red 100 | * 6 | white 101 | * 7 | brown 102 | * 103 | * 3. **ambient**\n 104 | * 'ambient' mode measures the ambient light and publishes into a **sensor_msgs::Illuminance** topic 105 | * 106 | * 4. **reflect**\n 107 | * 'reflect' mode measures the reflected light from an obstacle and publishes into a **sensor_msgs::Illuminance** topic 108 | * 109 | * \warning Currently **ambient** and **reflect** publish the direct sensor value (percent) in percent - this will change so that it fits the output value in the message 110 | * or the message type will be changed when it is found to be not right. 111 | * \todo check how to calculate LUX value for message or change message type 112 | */ 113 | if (!ctrl_nh.getParam("mode", mode_str)) 114 | { 115 | ROS_ERROR("Parameter mode was not set, using 'rgb_raw'"); 116 | } 117 | else 118 | { 119 | if(mode_str=="rgb_raw") 120 | { 121 | mode_=Ev3Strings::EV3COLORMODE_RGB_RAW; 122 | } 123 | else if(mode_str=="color") 124 | { 125 | mode_=Ev3Strings::EV3COLORMODE_COL_COLOR; 126 | } 127 | else if(mode_str=="ambient") 128 | { 129 | mode_=Ev3Strings::EV3COLORMODE_COL_AMBIENT; 130 | } 131 | else if(mode_str=="reflect") 132 | { 133 | mode_=Ev3Strings::EV3COLORMODE_COL_REFLECT; 134 | } 135 | else 136 | { 137 | ROS_ERROR_STREAM("Value for parameter mode unknown, only 'rgb_raw', 'color', 'reflect' and 'ambient' are supported!"); 138 | } 139 | } 140 | 141 | 142 | 143 | cout << "Port: " << port_ << endl; 144 | cout << "Mode: " << mode_str << endl; 145 | cout << "Publish rate: " << publish_rate_ << endl; 146 | 147 | handle_ = hw->getHandle(port_); 148 | color_interface_.setSensor(handle_.getSensor()); 149 | 150 | //TODO Mode handling 151 | 152 | if(!color_interface_.isConnected()) 153 | { 154 | 155 | ROS_ERROR_STREAM( 156 | "Need Color sensor on port: "<( 191 | root_nh, topic_name, 4)); 192 | realtime_illuminance_publisher_->msg_.header.frame_id=frame_id_; 193 | 194 | break; 195 | 196 | 197 | case Ev3Strings::EV3COLORMODE_COL_COLOR: 198 | topic_name=port_+"_color_number"; 199 | if (!ctrl_nh.getParam("topic_name", topic_name)) 200 | { 201 | ROS_INFO_STREAM("Parameter 'topic_name' not given, using "<( 206 | root_nh, topic_name, 4)); 207 | break; 208 | 209 | case Ev3Strings::EV3COLORMODE_RGB_RAW: 210 | topic_name=port_+"_color_rgb_raw"; 211 | if (!ctrl_nh.getParam("topic_name", topic_name)) 212 | { 213 | ROS_INFO_STREAM("Parameter 'topic_name' not given, using "<( 218 | root_nh, topic_name, 4)); 219 | break; 220 | 221 | 222 | default: 223 | ROS_ERROR_STREAM("Mode "< 0.0 292 | && last_publish_time_ + ros::Duration(1.0 / publish_rate_)< time) 293 | { 294 | bool published=false; 295 | switch(mode_) 296 | { 297 | case Ev3Strings::EV3COLORMODE_COL_AMBIENT: 298 | case Ev3Strings::EV3COLORMODE_COL_REFLECT: 299 | if (realtime_illuminance_publisher_->trylock()) 300 | { 301 | realtime_illuminance_publisher_->msg_.header.stamp = time; 302 | realtime_illuminance_publisher_->msg_.illuminance=value0; 303 | realtime_illuminance_publisher_->unlockAndPublish(); 304 | published=true; 305 | } 306 | break; 307 | 308 | 309 | case Ev3Strings::EV3COLORMODE_COL_COLOR: 310 | { 311 | realtime_color_number_publisher_->msg_.data=value0; 312 | realtime_color_number_publisher_->unlockAndPublish(); 313 | published=true; 314 | } 315 | break; 316 | 317 | case Ev3Strings::EV3COLORMODE_RGB_RAW: 318 | if (realtime_color_publisher_->trylock()) 319 | { 320 | realtime_color_publisher_->msg_.r=value0; 321 | realtime_color_publisher_->msg_.g=value1; 322 | realtime_color_publisher_->msg_.b=value2; 323 | realtime_color_publisher_->unlockAndPublish(); 324 | published=true; 325 | } 326 | 327 | } 328 | 329 | if(published) 330 | { 331 | last_publish_time_ = last_publish_time_ 332 | + ros::Duration(1.0 / publish_rate_); 333 | } 334 | } 335 | } 336 | 337 | PLUGINLIB_EXPORT_CLASS(ev3_control::Ev3ColorController, controller_interface::ControllerBase) 338 | 339 | } /* namespace ev3_control */ 340 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/ev3_gyro_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_gyro_controller.cpp) is part of h4r_ev3_control. 3 | * Date: 01.01.2016 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | #include 23 | 24 | namespace ev3_control 25 | { 26 | 27 | Ev3GyroController::Ev3GyroController() 28 | :sensor_mode_needs_init_(true) 29 | ,mode_(Ev3Strings::EV3GYROMODE_GYRO_ANG) 30 | ,publish_rate_(10) 31 | {} 32 | 33 | Ev3GyroController::~Ev3GyroController() 34 | {} 35 | 36 | 37 | bool Ev3GyroController::init(Ev3SensorInterface* hw, 38 | ros::NodeHandle &root_nh, 39 | ros::NodeHandle& ctrl_nh) 40 | { 41 | 42 | /** 43 | * 44 | * \page Ev3GyroController Ev3GyroController 45 | * \section Parameters 46 | * \subsection publish_rate 47 | * 48 | * The rate the controller should use to publish its messages. 49 | */ 50 | if (!ctrl_nh.getParam("publish_rate", publish_rate_)) 51 | { 52 | ROS_ERROR("Parameter publish_rate was not set, using 10 Hz"); 53 | } 54 | 55 | /** 56 | * \page Ev3GyroController Ev3GyroController 57 | * \subsection port 58 | * 59 | * 60 | * Specifies the EV3 port name. 61 | */ 62 | if (!ctrl_nh.getParam("port", port_)) 63 | { 64 | ROS_ERROR("Parameter port was not set"); 65 | return false; 66 | } 67 | 68 | std::string mode_str; 69 | 70 | /** 71 | * \page Ev3GyroController Ev3GyroController 72 | * \subsection mode 73 | * 74 | * The mode parameter sets the mode for the gyro sensor. If not set, it will use **angle**. 75 | * 1. **rate** 76 | * publishes the turn rate into a **sensor_msgs::Imu topic** (z-axis). 77 | * 2. **angle** 78 | * publishes the turn rate into a **sensor_msgs::Imu topic** (z-axis). 79 | * 3. **rate&angle** 80 | * publishes both turn and angle into a **sensor_msgs::Imu topic** (z-axis). 81 | * 82 | * \warning 83 | * **rate&angle** has, according to the EV3Dev manual, an instability. 84 | * If the angle reaches 32767 or -32768 it will be stuck there. 85 | * This is a sensor issue. 86 | * Using this mode is not recommended. 87 | */ 88 | if (!ctrl_nh.getParam("mode", mode_str)) 89 | { 90 | ROS_ERROR("Parameter mode was not set, using 'angle'"); 91 | } 92 | else 93 | { 94 | if(mode_str=="rate") 95 | { 96 | mode_=Ev3Strings::EV3GYROMODE_GYRO_RATE; 97 | } 98 | else if(mode_str=="angle") 99 | { 100 | mode_=Ev3Strings::EV3GYROMODE_GYRO_ANG; 101 | } 102 | else if(mode_str=="rate&angle") 103 | { 104 | mode_=Ev3Strings::EV3GYROMODE_GYRO_G_A; 105 | ROS_WARN("Setting rate and angle mode! According to documentation of EV3Dev " 106 | "this mode is instable, when the value of the angle reaches ~32000 it crashes the driver!"); 107 | } 108 | else 109 | { 110 | ROS_ERROR_STREAM("Value for parameter mode unknown supported are: angle, rate" ); 111 | } 112 | } 113 | 114 | 115 | 116 | cout << "Port: " << port_ << endl; 117 | cout << "Mode: " << mode_str << endl; 118 | cout << "Publish rate: " << publish_rate_ << endl; 119 | 120 | 121 | handle_ = hw->getHandle(port_); 122 | gyro_interface_.setSensor(handle_.getSensor()); 123 | 124 | if(!gyro_interface_.isConnected()) 125 | { 126 | 127 | ROS_ERROR_STREAM( 128 | "Need gyro sensor on port: "<( 160 | root_nh, topic_name, 4)); 161 | 162 | realtime_imu_publisher_->msg_.header.frame_id=frame_id_; 163 | //Init orientation to be a legal value for rate mode 164 | realtime_imu_publisher_->msg_.orientation=tf::createQuaternionMsgFromYaw( 0.0 ); 165 | 166 | return true; 167 | } 168 | 169 | void Ev3GyroController::starting(const ros::Time& time) 170 | { 171 | last_publish_time_ = time; 172 | } 173 | 174 | void Ev3GyroController::update(const ros::Time& time, const ros::Duration& /*period*/) 175 | { 176 | using namespace hardware_interface; 177 | 178 | int value0,value1; 179 | 180 | 181 | if(!gyro_interface_.isConnected()) 182 | { 183 | sensor_mode_needs_init_=true; 184 | return; 185 | } 186 | 187 | if(sensor_mode_needs_init_) 188 | { 189 | sensor_mode_needs_init_=!(gyro_interface_.setMode(mode_)); 190 | if(sensor_mode_needs_init_) 191 | { 192 | ROS_ERROR("Could not set sensor mode!"); 193 | } 194 | } 195 | 196 | if (!handle_.getValue(0, value0))return; 197 | 198 | if (mode_==Ev3Strings::EV3GYROMODE_GYRO_G_A) 199 | { 200 | if (!handle_.getValue(1, value1))return; 201 | } 202 | 203 | if (publish_rate_ > 0.0 204 | && last_publish_time_ + ros::Duration(1.0 / publish_rate_)< time) 205 | { 206 | bool published=false; 207 | 208 | if (realtime_imu_publisher_->trylock()) 209 | { 210 | realtime_imu_publisher_->msg_.header.stamp = time; 211 | 212 | switch(mode_) 213 | { 214 | 215 | case Ev3Strings::EV3GYROMODE_GYRO_ANG: 216 | realtime_imu_publisher_->msg_.orientation=tf::createQuaternionMsgFromYaw( ((double)value0 ) * M_PI/180.0); 217 | break; 218 | 219 | case Ev3Strings::EV3GYROMODE_GYRO_RATE: 220 | realtime_imu_publisher_->msg_.angular_velocity.z=( (double) value0 ) * M_PI/180.0; 221 | break; 222 | 223 | case Ev3Strings::EV3GYROMODE_GYRO_G_A: 224 | realtime_imu_publisher_->msg_.orientation=tf::createQuaternionMsgFromYaw( ((double) value0 ) * M_PI/180.0); 225 | realtime_imu_publisher_->msg_.angular_velocity.z=( (double) value1 ) * M_PI/180.0; 226 | break; 227 | 228 | } 229 | 230 | realtime_imu_publisher_->unlockAndPublish(); 231 | published=true; 232 | } 233 | 234 | 235 | if(published) 236 | { 237 | last_publish_time_ = last_publish_time_ 238 | + ros::Duration(1.0 / publish_rate_); 239 | } 240 | 241 | } 242 | } 243 | 244 | PLUGINLIB_EXPORT_CLASS(ev3_control::Ev3GyroController, controller_interface::ControllerBase) 245 | 246 | } /* namespace ev3_control */ 247 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/ev3_infrared_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_ultrasonic_controller.cpp) is part of h4r_ev3_control. 3 | * Date: 17.12.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace ev3_control 26 | { 27 | 28 | Ev3InfraredController::Ev3InfraredController() 29 | :sensor_mode_needs_init_(true) 30 | ,mode_(Ev3Strings::EV3INFRAREDMODE_IR_PROX) 31 | ,max_range_(2.0) 32 | ,min_range_(0) 33 | ,publish_rate_(10) 34 | { 35 | // TODO Auto-generated constructor stub 36 | 37 | } 38 | 39 | Ev3InfraredController::~Ev3InfraredController() 40 | { 41 | // TODO Auto-generated destructor stub 42 | } 43 | 44 | 45 | bool Ev3InfraredController::init(Ev3SensorInterface* hw, 46 | ros::NodeHandle &root_nh, 47 | ros::NodeHandle& ctrl_nh) 48 | 49 | { 50 | 51 | for (int i = 0; i < 4; ++i) { 52 | first_time_[i]=true; 53 | } 54 | 55 | /** 56 | * 57 | * \page Ev3InfraredController Ev3InfraredController 58 | * \section Parameters 59 | * \subsection publish_rate 60 | * 61 | * The rate which the controller should use to publish its messages. 62 | */ 63 | if (!ctrl_nh.getParam("publish_rate", publish_rate_)) 64 | { 65 | ROS_ERROR("Parameter publish_rate was not set, using 10 Hz"); 66 | } 67 | 68 | 69 | /** 70 | * \page Ev3InfraredController Ev3InfraredController 71 | * \subsection port 72 | * 73 | * 74 | * Specifies the EV3 port name. 75 | */ 76 | if (!ctrl_nh.getParam("port", port_)) 77 | { 78 | ROS_ERROR("Parameter port was not set"); 79 | return false; 80 | } 81 | 82 | /** 83 | * \page Ev3InfraredController Ev3InfraredController 84 | * \subsection mode 85 | * 1. **proximity** publishes the distance of the sensor to a reflective surface. 86 | * 87 | * 2. **seek** publishes four topics containing the heading and the distance 88 | * value to ir-remotes in beacon mode for each remote channel 89 | * 90 | * 3. **remote** 91 | * publishes the four remote channels of the ir-sensor to four different topics from type **sensor_msgs::Joy**\n 92 | * The buttons in the joy topic correlate to the following array element: 93 | * Element | Button 94 | * --------|------- 95 | * 0 | red up 96 | * 1 | red down 97 | * 2 | blue up 98 | * 3 | blue down 99 | * 4 | beacon 100 | * 101 | * 102 | */ 103 | std::string mode_str; 104 | if (!ctrl_nh.getParam("mode", mode_str)) 105 | { 106 | ROS_ERROR("Parameter mode was not set, using 'proximity'"); 107 | } 108 | else 109 | { 110 | if(mode_str=="proximity") 111 | { 112 | mode_=Ev3Strings::EV3INFRAREDMODE_IR_PROX; 113 | } 114 | else if(mode_str=="seek") 115 | { 116 | mode_=Ev3Strings::EV3INFRAREDMODE_IR_SEEK; 117 | } 118 | else if(mode_str=="remote") 119 | { 120 | mode_=Ev3Strings::EV3INFRAREDMODE_IR_REMOTE; 121 | } 122 | else 123 | { 124 | ROS_ERROR_STREAM("Value for parameter mode unknown, only 'proximity' and 'seek' are supported!"); 125 | } 126 | } 127 | 128 | 129 | 130 | cout << "Port: " << port_ << endl; 131 | cout << "Mode: " << mode_str << endl; 132 | cout << "Publish rate: " << publish_rate_ << endl; 133 | 134 | 135 | 136 | handle_ = hw->getHandle(port_); 137 | ir_interface_.setSensor(handle_.getSensor()); 138 | 139 | 140 | //TODO Mode handling 141 | 142 | if(!ir_interface_.isConnected()) 143 | { 144 | 145 | ROS_ERROR_STREAM( 146 | "Need Infrared Sensor on port: "<( 188 | root_nh, topic_name, 4)); 189 | 190 | 191 | 192 | 193 | if (!ctrl_nh.getParam("max_range", max_range_)) 194 | { 195 | ROS_INFO_STREAM("Parameter max_range not given, using 2.0"); 196 | } 197 | 198 | if (!ctrl_nh.getParam("min_range", min_range_)) 199 | { 200 | ROS_INFO_STREAM("Parameter min_range not given or wrong type, using 0"); 201 | } 202 | 203 | 204 | 205 | if(max_range_>2.50) 206 | { 207 | ROS_ERROR("Parameter max_range to big! 2.550 is error condition of the sensor using 2.5"); 208 | max_range_=2.5; 209 | } 210 | 211 | realtime_range_publisher_->msg_.min_range=min_range_; 212 | realtime_range_publisher_->msg_.max_range=max_range_; 213 | realtime_range_publisher_->msg_.radiation_type=1; //INFRARED 214 | realtime_range_publisher_->msg_.header.frame_id=frame_id_; 215 | realtime_range_publisher_->msg_.field_of_view=0.872665; //50deg (in rads) 216 | break; 217 | 218 | case Ev3Strings::EV3INFRAREDMODE_IR_SEEK: 219 | topic_name=port_+"_ir_seek"; 220 | 221 | 222 | std::cout<<"Seek Mode Setup!"<( 235 | root_nh, topic_name, 4)); 236 | 237 | } 238 | break; 239 | 240 | case Ev3Strings::EV3INFRAREDMODE_IR_REMOTE: 241 | topic_name=port_+"_ir_remote"; 242 | 243 | 244 | std::cout<<"Remote Mode Setup!"<( 261 | root_nh, topic_name, 4,true)); 262 | realtime_joy_publishers_[i]->msg_.buttons.resize(5); 263 | realtime_joy_publishers_[i]->msg_.header.frame_id=frame_id_; 264 | } 265 | break; 266 | 267 | 268 | 269 | default: 270 | ROS_ERROR_STREAM("Mode "< 0.0 343 | && last_publish_time_[0] + ros::Duration(1.0 / publish_rate_)< time) 344 | { 345 | 346 | if (realtime_range_publisher_->trylock()) 347 | { 348 | realtime_range_publisher_->msg_.header.stamp = time; 349 | 350 | if(value[0]!=2550) 351 | { 352 | double dval = ((double) value[0]) / 1000.0; //to meters 353 | 354 | if(dval < min_range_) 355 | { 356 | dval=-std::numeric_limits::infinity(); 357 | } 358 | else if(dval > max_range_) 359 | { 360 | dval=std::numeric_limits::infinity(); 361 | } 362 | 363 | realtime_range_publisher_->msg_.range=dval; 364 | } 365 | else 366 | { 367 | //value==2550 means no response (either covered sensor or too far away) 368 | realtime_range_publisher_->msg_.range = std::numeric_limits::infinity(); 369 | } 370 | 371 | realtime_range_publisher_->unlockAndPublish(); 372 | published[0]=true; 373 | } 374 | } 375 | break; 376 | 377 | case Ev3Strings::EV3INFRAREDMODE_IR_SEEK: 378 | for (int i = 0; i < 4; ++i) 379 | { 380 | if (publish_rate_ > 0.0 381 | && last_publish_time_[i] + ros::Duration(1.0 / publish_rate_)< time) 382 | { 383 | if (realtime_seek_publishers_[i]->trylock()) 384 | { 385 | double distance=value[i*2+1]; 386 | double heading=value[i*2]; 387 | 388 | if(distance>=100.0) 389 | { 390 | realtime_seek_publishers_[i]->msg_.distance=std::numeric_limits::infinity(); 391 | realtime_seek_publishers_[i]->msg_.heading=std::numeric_limits::quiet_NaN(); 392 | } 393 | else if(distance>=0.0) 394 | { 395 | realtime_seek_publishers_[i]->msg_.distance=distance; 396 | realtime_seek_publishers_[i]->msg_.heading=heading; 397 | } 398 | else //distance<0 399 | { 400 | realtime_seek_publishers_[i]->msg_.distance=std::numeric_limits::quiet_NaN(); 401 | } 402 | 403 | realtime_seek_publishers_[i]->unlockAndPublish(); 404 | published[i]=true; 405 | } 406 | } 407 | 408 | } 409 | break; 410 | 411 | case Ev3Strings::EV3INFRAREDMODE_IR_REMOTE: 412 | for (int i = 0; i < 4; ++i) 413 | { 414 | if (publish_rate_ > 0.0 415 | && last_publish_time_[i] + ros::Duration(1.0 / publish_rate_)< time) 416 | { 417 | if (realtime_joy_publishers_[i]->trylock()) 418 | { 419 | 420 | realtime_joy_publishers_[i]->msg_.header.stamp=ros::Time::now(); 421 | 422 | bool red_up_pressed= 423 | value[i]==1 424 | || value[i]==5 425 | || value[i]==6 426 | || value[i]==10; 427 | 428 | bool red_down_pressed= 429 | value[i]==2 430 | || value[i]==7 431 | || value[i]==8 432 | || value[i]==10; 433 | 434 | bool blue_up_pressed= 435 | value[i]==3 436 | || value[i]==5 437 | || value[i]==7 438 | || value[i]==11; 439 | bool blue_down_pressed= 440 | value[i]==4 441 | || value[i]==6 442 | || value[i]==8 443 | || value[i]==11; 444 | 445 | bool beacon_mode_active= 446 | value[i]==9; 447 | 448 | 449 | //Only publish on change otherwise assume published and end... 450 | if( 451 | realtime_joy_publishers_[i]->msg_.buttons[0]==red_up_pressed&& 452 | realtime_joy_publishers_[i]->msg_.buttons[1]==red_down_pressed&& 453 | realtime_joy_publishers_[i]->msg_.buttons[2]==blue_up_pressed&& 454 | realtime_joy_publishers_[i]->msg_.buttons[3]==blue_down_pressed&& 455 | realtime_joy_publishers_[i]->msg_.buttons[4]==beacon_mode_active&& 456 | !first_time_[i] 457 | ) 458 | { 459 | realtime_joy_publishers_[i]->unlock(); 460 | published[i]=true; 461 | } 462 | else 463 | { 464 | 465 | realtime_joy_publishers_[i]->msg_.buttons[0]=red_up_pressed; 466 | realtime_joy_publishers_[i]->msg_.buttons[1]=red_down_pressed; 467 | realtime_joy_publishers_[i]->msg_.buttons[2]=blue_up_pressed; 468 | realtime_joy_publishers_[i]->msg_.buttons[3]=blue_down_pressed; 469 | realtime_joy_publishers_[i]->msg_.buttons[4]=beacon_mode_active; 470 | 471 | realtime_joy_publishers_[i]->unlockAndPublish(); 472 | published[i]=true; 473 | first_time_[i]=false; 474 | } 475 | 476 | } 477 | } 478 | } 479 | 480 | break; 481 | 482 | default: 483 | break; 484 | } 485 | 486 | for (int i = 0; i < 4; ++i) 487 | { 488 | if(published[i]) 489 | { 490 | last_publish_time_[i] = last_publish_time_[i] 491 | + ros::Duration(1.0 / publish_rate_); 492 | } 493 | } 494 | 495 | 496 | } 497 | 498 | PLUGINLIB_EXPORT_CLASS(ev3_control::Ev3InfraredController, controller_interface::ControllerBase) 499 | 500 | } /* namespace ev3_control */ 501 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/ev3_touch_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_touch_controller.cpp) is part of h4r_ev3_control. 3 | * Date: 01.01.2016 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | #include 23 | 24 | namespace ev3_control 25 | { 26 | 27 | Ev3TouchController::Ev3TouchController() 28 | :sensor_mode_needs_init_(true) 29 | ,publish_rate_(10) 30 | {} 31 | 32 | Ev3TouchController::~Ev3TouchController() 33 | {} 34 | 35 | 36 | bool Ev3TouchController::init(Ev3SensorInterface* hw, 37 | ros::NodeHandle &root_nh, 38 | ros::NodeHandle& ctrl_nh) 39 | { 40 | 41 | if (!ctrl_nh.getParam("publish_rate", publish_rate_)) 42 | { 43 | ROS_ERROR("Parameter publish_rate was not set, using 10 Hz"); 44 | } 45 | 46 | if (!ctrl_nh.getParam("port", port_)) 47 | { 48 | ROS_ERROR("Parameter port was not set"); 49 | return false; 50 | } 51 | 52 | 53 | 54 | cout << "Port: " << port_ << endl; 55 | cout << "Publish rate: " << publish_rate_ << endl; 56 | 57 | 58 | handle_ = hw->getHandle(port_); 59 | touch_interface_.setSensor(handle_.getSensor()); 60 | 61 | if(!touch_interface_.isConnected()) 62 | { 63 | 64 | ROS_ERROR_STREAM( 65 | "Need touch sensor on port: "<( 85 | root_nh, topic_name, 4)); 86 | 87 | 88 | return true; 89 | } 90 | 91 | void Ev3TouchController::starting(const ros::Time& time) 92 | { 93 | last_publish_time_ = time; 94 | } 95 | 96 | void Ev3TouchController::update(const ros::Time& time, const ros::Duration& /*period*/) 97 | { 98 | using namespace hardware_interface; 99 | 100 | int value0,value1; 101 | 102 | 103 | if(!touch_interface_.isConnected()) 104 | { 105 | sensor_mode_needs_init_=true; 106 | return; 107 | } 108 | 109 | if (!handle_.getValue(0, value0))return; 110 | 111 | 112 | if (publish_rate_ > 0.0 113 | && last_publish_time_ + ros::Duration(1.0 / publish_rate_)< time) 114 | { 115 | bool published=false; 116 | 117 | if (realtime_bool_publisher_->trylock()) 118 | { 119 | realtime_bool_publisher_->msg_.data=(double) (!!value0); 120 | 121 | realtime_bool_publisher_->unlockAndPublish(); 122 | published=true; 123 | } 124 | 125 | 126 | if(published) 127 | { 128 | last_publish_time_ = last_publish_time_ 129 | + ros::Duration(1.0 / publish_rate_); 130 | } 131 | 132 | } 133 | } 134 | 135 | PLUGINLIB_EXPORT_CLASS(ev3_control::Ev3TouchController, controller_interface::ControllerBase) 136 | 137 | } /* namespace ev3_control */ 138 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/ev3_ultrasonic_controller.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_ultrasonic_controller.cpp) is part of h4r_ev3_control. 3 | * Date: 17.12.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace ev3_control 26 | { 27 | 28 | Ev3UltrasonicController::Ev3UltrasonicController() 29 | :sensor_mode_needs_init_(true) 30 | ,mode_(Ev3Strings::EV3ULTRASONICMODE_US_DIST_CM) 31 | ,max_range_(2.0) 32 | ,min_range_(0) 33 | ,publish_rate_(10) 34 | { 35 | // TODO Auto-generated constructor stub 36 | 37 | } 38 | 39 | Ev3UltrasonicController::~Ev3UltrasonicController() 40 | { 41 | // TODO Auto-generated destructor stub 42 | } 43 | 44 | 45 | bool Ev3UltrasonicController::init(Ev3SensorInterface* hw, 46 | ros::NodeHandle &root_nh, 47 | ros::NodeHandle& ctrl_nh) 48 | { 49 | 50 | /** 51 | * 52 | * \page Ev3UltrasonicController Ev3UltrasonicController 53 | * \section Parameters 54 | * \subsection publish_rate 55 | * 56 | * The rate the controller should use to publish its messages. 57 | */ 58 | if (!ctrl_nh.getParam("publish_rate", publish_rate_)) 59 | { 60 | ROS_ERROR("Parameter publish_rate was not set, using 10 Hz"); 61 | } 62 | 63 | /** 64 | * \page Ev3UltrasonicController Ev3UltrasonicController 65 | * \subsection port 66 | * 67 | * 68 | * Specifies the EV3 port name. 69 | */ 70 | if (!ctrl_nh.getParam("port", port_)) 71 | { 72 | ROS_ERROR("Parameter port was not set"); 73 | return false; 74 | } 75 | 76 | std::string mode_str; 77 | /** 78 | * \page Ev3UltrasonicController Ev3UltrasonicController 79 | * \subsection mode 80 | * 1. **distance** publishes the measured distance into a **sensor_msgs::Range** topic 81 | * 2. **listen** publishes into a **std_msgs::Boolean** topic, when it detects 82 | * another Ultrasonic Sensor or a loud noise (like a clap) the value is true otherwise false. 83 | * - **max_range** sets the maximum range of values taken into account if a value is bigger it will be replaced by infinity.\n 84 | * The maximum value for this parameter is 2.5 because the value 2.550 is the sensors error condition. 85 | * 86 | * - **min_range** sets the minimum range of values taken into account if a value is smaller it will be replaced by negative infinity. 87 | */ 88 | if (!ctrl_nh.getParam("mode", mode_str)) 89 | { 90 | ROS_ERROR("Parameter mode was not set, using 'distance'"); 91 | mode_=Ev3Strings::EV3ULTRASONICMODE_US_DIST_CM; 92 | } 93 | else 94 | { 95 | if(mode_str=="distance") 96 | { 97 | mode_=Ev3Strings::EV3ULTRASONICMODE_US_DIST_CM; 98 | } 99 | else if(mode_str=="listen") 100 | { 101 | mode_=Ev3Strings::EV3ULTRASONICMODE_US_LISTEN; 102 | } 103 | else 104 | { 105 | ROS_ERROR_STREAM("Value for parameter mode unknown, only 'distance' and 'seek' are supported!"); 106 | } 107 | } 108 | 109 | 110 | 111 | cout << "Port: " << port_ << endl; 112 | cout << "Mode: " << mode_str << endl; 113 | cout << "Publish rate: " << publish_rate_ << endl; 114 | 115 | 116 | 117 | handle_ = hw->getHandle(port_); 118 | us_interface_.setSensor(handle_.getSensor()); 119 | 120 | //TODO Mode handling 121 | 122 | if(!us_interface_.isConnected()) 123 | { 124 | 125 | ROS_ERROR_STREAM( 126 | "Need Ultrasonic Sensor on port: "<( 146 | root_nh, topic_name, 4)); 147 | 148 | 149 | 150 | 151 | if (!ctrl_nh.getParam("max_range", max_range_)) 152 | { 153 | ROS_INFO_STREAM("Parameter max_range not given, using 2.0"); 154 | } 155 | 156 | if (!ctrl_nh.getParam("min_range", min_range_)) 157 | { 158 | ROS_INFO_STREAM("Parameter min_range not given or wrong type, using 0"); 159 | } 160 | 161 | 162 | 163 | 164 | if (!ctrl_nh.getParam("frame_id", frame_id_)) 165 | { 166 | frame_id_=port_; 167 | ROS_INFO_STREAM("Parameter frame_id not given or wrong type, using "<2.50) 171 | { 172 | ROS_ERROR("Parameter max_range to big! 2.550 is error condition of the sensor using 2.5"); 173 | max_range_=2.5; 174 | } 175 | 176 | realtime_range_publisher_->msg_.min_range=min_range_; 177 | realtime_range_publisher_->msg_.max_range=max_range_; 178 | realtime_range_publisher_->msg_.radiation_type=0; //ULTRASONIC 179 | realtime_range_publisher_->msg_.header.frame_id=frame_id_; 180 | realtime_range_publisher_->msg_.field_of_view=0.872665; //50deg (in rads) 181 | break; 182 | 183 | case Ev3Strings::EV3ULTRASONICMODE_US_LISTEN: 184 | topic_name=port_+"_us_listen"; 185 | if (!ctrl_nh.getParam("topic_name", topic_name)) 186 | { 187 | ROS_INFO_STREAM("Parameter topic name not given using"<( 194 | root_nh, topic_name, 4)); 195 | break; 196 | 197 | default: 198 | ROS_ERROR_STREAM("Mode "< 0.0 243 | && last_publish_time_ + ros::Duration(1.0 / publish_rate_)< time) 244 | { 245 | bool published=false; 246 | switch(mode_) 247 | { 248 | case Ev3Strings::EV3ULTRASONICMODE_US_DIST_CM: 249 | case Ev3Strings::EV3ULTRASONICMODE_US_DC_CM: 250 | if (realtime_range_publisher_->trylock()) 251 | { 252 | realtime_range_publisher_->msg_.header.stamp = time; 253 | 254 | if(value!=2550) 255 | { 256 | double dval = ((double) value) / 1000.0; //to meters 257 | 258 | if(dval < min_range_) 259 | { 260 | dval=-std::numeric_limits::infinity(); 261 | } 262 | else if(dval > max_range_) 263 | { 264 | dval=std::numeric_limits::infinity(); 265 | } 266 | 267 | realtime_range_publisher_->msg_.range=dval; 268 | } 269 | else 270 | { 271 | //value==2550 means no response (either covered sensor or too far away) 272 | realtime_range_publisher_->msg_.range = std::numeric_limits::infinity(); 273 | } 274 | 275 | realtime_range_publisher_->unlockAndPublish(); 276 | published=true; 277 | } 278 | break; 279 | 280 | case Ev3Strings::EV3ULTRASONICMODE_US_LISTEN: 281 | if (realtime_bool_publisher_->trylock()) 282 | { 283 | realtime_bool_publisher_->msg_.data=(bool)value; 284 | realtime_bool_publisher_->unlockAndPublish(); 285 | published=true; 286 | } 287 | break; 288 | 289 | default: 290 | break; 291 | } 292 | 293 | 294 | if(published) 295 | { 296 | last_publish_time_ = last_publish_time_ 297 | + ros::Duration(1.0 / publish_rate_); 298 | } 299 | 300 | } 301 | } 302 | 303 | PLUGINLIB_EXPORT_CLASS(ev3_control::Ev3UltrasonicController, controller_interface::ControllerBase) 304 | 305 | } /* namespace ev3_control */ 306 | -------------------------------------------------------------------------------- /h4r_ev3_control/src/h4r_ev3_control/syshelpers.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (syshelpers.cpp) is part of h4r_ev3_control. 3 | * Date: 22.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_control is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_control is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | #include 23 | #include 24 | namespace ev3_control 25 | { 26 | 27 | bool pathExists(const char* path) 28 | { 29 | return (access( path, F_OK ) != -1); 30 | } 31 | 32 | bool writeIntToSysFile(FILE *fileptr,int value) 33 | { 34 | std::string out; 35 | bool negative= (value<0); 36 | for (int i = 0; value >0 ; ++i) 37 | { 38 | out+=(value%10)+'0'; 39 | value/=10; 40 | } 41 | if(negative) 42 | out+='-'; 43 | 44 | out=std::string ( out.rbegin(), out.rend() ); 45 | out+='\n'; 46 | 47 | 48 | int wrote=fwrite(out.c_str(),1,out.size(),fileptr); 49 | 50 | 51 | if(fflush(fileptr)) 52 | return false; 53 | rewind(fileptr); 54 | 55 | return wrote==out.size(); 56 | } 57 | 58 | bool readIntFromSysFile(FILE *fileptr, int &value) 59 | { 60 | fflush(fileptr); 61 | rewind(fileptr); 62 | int64_t value_out; 63 | value=0; 64 | 65 | char buffer[256]; 66 | char *buf=&buffer[0]; 67 | char **bufptr=&buf; 68 | 69 | bool negative; 70 | ssize_t read; 71 | size_t len=256; 72 | int l=0; 73 | while ((read = getline(bufptr, &len, fileptr)) != -1) { 74 | if(l==0) 75 | { 76 | bool negative=false; 77 | for (int b = 0; b < read; ++b) 78 | { 79 | if(buffer[b]=='\n') 80 | { 81 | break; //Just in case... 82 | } 83 | 84 | value*=10; 85 | switch(buffer[b]) 86 | { 87 | case '-': 88 | if(b==0) 89 | { 90 | negative=true; 91 | } 92 | else 93 | { 94 | return false; 95 | } 96 | break; 97 | 98 | case '0': 99 | case '1': 100 | case '2': 101 | case '3': 102 | case '4': 103 | case '5': 104 | case '6': 105 | case '7': 106 | case '8': 107 | case '9': 108 | value+=buffer[b]-48; 109 | break; 110 | default: 111 | return false; 112 | 113 | } 114 | } 115 | if(negative) 116 | value*=-1; 117 | return true; 118 | } 119 | else 120 | { 121 | return false; 122 | } 123 | } 124 | return true; 125 | } 126 | 127 | bool matchFileContentInEqualSubdirectories(const char* parent, const char* file, 128 | const char* content, FileNameBuffer &match_dir) 129 | { 130 | 131 | bool found = false; 132 | DIR *dp; 133 | struct dirent *subdir; 134 | 135 | dp = opendir(parent); 136 | 137 | if (dp != NULL) 138 | { 139 | while (subdir = readdir(dp)) 140 | { 141 | std::string directory; 142 | directory += subdir->d_name; 143 | 144 | if (directory != "." && directory != "..") 145 | { 146 | 147 | match_dir.format("%s/%s/%s", parent, subdir->d_name, file); 148 | 149 | FILE *fileptr = fopen(match_dir.c_str(), "r"); 150 | 151 | if (fileptr == NULL) 152 | continue; 153 | 154 | int64_t value_out; 155 | 156 | char buffer[256]; 157 | char *buf = &buffer[0]; 158 | char **bufptr = &buf; 159 | 160 | bool negative; 161 | ssize_t read; 162 | size_t len = 256; 163 | int l = 0; 164 | 165 | while ((read = getline(bufptr, &len, fileptr)) != -1) 166 | { 167 | if (l == 0) 168 | { 169 | buffer[read - 1] = 0x00; //remove linefeed! 170 | if (strcmp(content, buffer) == 0) 171 | { 172 | match_dir.format("%s/%s", parent, subdir->d_name); 173 | found = true; 174 | break; 175 | } 176 | } 177 | else 178 | { 179 | break; 180 | } 181 | } 182 | 183 | fclose(fileptr); 184 | } 185 | if (found) 186 | break; 187 | } 188 | closedir(dp); 189 | 190 | return found; 191 | } 192 | else 193 | { 194 | return false; 195 | } 196 | } 197 | 198 | 199 | }/*ev3_control*/ 200 | -------------------------------------------------------------------------------- /h4r_ev3_control/string_gen/generateMaps.py: -------------------------------------------------------------------------------- 1 | import yaml 2 | import re 3 | import os 4 | from Crypto.Util.number import size 5 | stream = file('strings.yml', 'r') 6 | document=yaml.load(stream) 7 | 8 | entries=[] 9 | for type in document['strings']: 10 | enum_entries=[] 11 | strings=[] 12 | 13 | #Enum Class (CamelCase) 14 | type_class="" 15 | for p in re.split("_", type): 16 | type_class+=p.title() 17 | 18 | 19 | for string in document['strings'][type]: 20 | if(document['strings'][type][string]==None): 21 | enum_entries.append(type_class.upper()+"_"+re.sub("-", "_", string).upper()) 22 | else: 23 | enum_entries.append(type_class.upper()+"_"+document['strings'][type][string].upper()) 24 | strings.append(string) 25 | 26 | 27 | #print type 28 | #print type_class 29 | #print enum_entries 30 | #print strings 31 | 32 | entries.append([type, type_class, enum_entries, strings]) 33 | 34 | 35 | output_header="\n/*WARNING WARNING WARNING WARNING WARNING" 36 | output_header+="\n *This file is generated by script" 37 | output_header+="\n *generateMaps.py, to add strings change" 38 | output_header+="\n *strings.yml and run it again!" 39 | output_header+="\n *WARNING WARNING WARNING WARNING WARNING\n" 40 | output_header+=" */\n" 41 | 42 | 43 | output_header+="#ifndef EV3STRINGS_H\n" 44 | output_header+="#define EV3STRINGS_H\n" 45 | 46 | output_header+=" #include \n" 47 | #output_header+=" using namespace std;\n\n" 48 | 49 | output_header+="namespace ev3_control{\n" 50 | output_header+="class Ev3Strings\n{\n" 51 | 52 | output_header+="private: \n" 53 | output_header+="Ev3Strings(){}/*thou shalt never construct this!*/\n" 54 | 55 | 56 | 57 | output_header+="public:\n" 58 | 59 | for entry in entries: 60 | output_header+="\ttypedef enum\n" 61 | output_header+= "\t{\n" 62 | output_header+= "\t\t" + re.sub("-", "_", entry[1]).upper()+"_NOT_FOUND=-1,\n" 63 | for e in entry[2]: 64 | output_header+="\t\t"+ e + ",\n" 65 | output_header+="\t}" + entry[1] +";\n\n" 66 | 67 | 68 | output_header+="\tstatic StringEnum<"+entry[1]+"> "+ entry[0]+"_conv;\n\n\n" 69 | 70 | # output_header+="\tstatic " + entry[1] +" "+entry[1] + "FromString(const string& str);\n" 71 | # output_header+="\tstatic string "+entry[1]+"ToString("+entry[1]+" val);\n\n" 72 | 73 | # output_header+="\tstatic const map<"+ entry[1] +",string> " + entry[0] +"_string;\n" 74 | # output_header+="\tstatic const map " + entry[0] +"_enum;\n\n" 75 | 76 | 77 | output_header+="};\n" 78 | output_header+="}\n" 79 | 80 | output_header+="#endif" 81 | output_header+="\n\n\n" 82 | 83 | 84 | 85 | output_source="\n/*WARNING WARNING WARNING WARNING WARNING" 86 | output_source+="\n *This file is generated by script" 87 | output_source+="\n *generateMaps.py, to add strings change" 88 | output_source+="\n *strings.yml and run it again!" 89 | output_source+="\n *WARNING WARNING WARNING WARNING WARNING\n" 90 | output_source+=" */\n" 91 | 92 | output_source+=" #include \n" 93 | output_source+="namespace ev3_control{\n" 94 | 95 | 96 | for entry in entries: 97 | 98 | ##Variable Init Function Strings 99 | 100 | output_source+="StringEnum init_"+ entry[0] +"_conv()\n" 101 | output_source+="{\n" 102 | output_source+="\tStringEnum mp;\n" 103 | 104 | 105 | for e in range(len(entry[2])): 106 | output_source+="\tmp.insert(\""+entry[3][e]+"\");\n" 107 | 108 | output_source+="\tmp.finalize();\n" 109 | output_source+="\treturn mp;\n" 110 | output_source+="}\n\n" 111 | 112 | # ##Variable Init Function Enum 113 | # 114 | # output_source+="map init_"+ entry[0] +"_enum_map()\n" 115 | # output_source+="{\n" 116 | # output_source+="\tmap mp;\n" 117 | # 118 | # 119 | # for e in range(len(entry[2])): 120 | # output_source+="\tmp.insert(pair(\""+entry[3][e]+"\",Ev3Strings::"+entry[2][e]+"));\n" 121 | # 122 | # 123 | # output_source+="\treturn mp;\n" 124 | # output_source+="}\n\n" 125 | # 126 | # output_source+="Ev3Strings::"+entry[1] +" Ev3Strings::"+entry[1] + "FromString(const string& str)\n" 127 | # output_source+="{\n" 128 | # output_source+="\tmap::const_iterator it=Ev3Strings::"+ entry[0] +"_enum.find(str);\n" 129 | # output_source+="\tif(it!=Ev3Strings::"+entry[0]+"_enum.end())\n" 130 | # output_source+="\t{return it->second;}\n" 131 | # 132 | # output_source+="\treturn "+entry[1].upper()+"_NOT_FOUND;\n" 133 | # output_source+="}\n\n" 134 | # 135 | # output_source+="string Ev3Strings::"+entry[1]+"ToString("+entry[1]+" val)\n" 136 | # output_source+="{\n" 137 | # output_source+="\tmap::const_iterator it=Ev3Strings::"+ entry[0] +"_string.find(val);\n" 138 | # output_source+="\tif(it!=Ev3Strings::"+entry[0]+"_string.end()){return it->second;}\n\n" 139 | # 140 | # output_source+="\treturn \"\";\n"; 141 | # output_source+="}\n" 142 | # 143 | ##Variable 144 | 145 | output_source+="StringEnumEv3Strings::" + entry[0] + "_conv = init_"+ entry[0] +"_conv();\n" 146 | # output_source+="const mapEv3Strings::" + entry[0] + "_enum= init_"+ entry[0] +"_enum_map();\n" 147 | 148 | output_source+="\n\n\n" 149 | 150 | output_source+="}\n" 151 | 152 | 153 | directory="../include/h4r_ev3_control/" 154 | 155 | if not os.path.exists(directory): 156 | os.makedirs(directory) 157 | 158 | directory="../src/h4r_ev3_control/" 159 | 160 | if not os.path.exists(directory): 161 | os.makedirs(directory) 162 | 163 | 164 | 165 | header_file = open("../include/h4r_ev3_control/Ev3Strings.h", "w") 166 | header_file.write(output_header) 167 | header_file.close() 168 | 169 | source_file = open("../src/h4r_ev3_control/Ev3Strings.cpp", "w") 170 | source_file.write(output_source) 171 | source_file.close() 172 | -------------------------------------------------------------------------------- /h4r_ev3_control/string_gen/strings.yml: -------------------------------------------------------------------------------- 1 | strings: 2 | #Port Drivers 3 | ev3_port_drivers: 4 | legoev3-input-port: 5 | legoev3-input-outport: 6 | 7 | #Port modes 8 | ev3_port_status: 9 | no-connect: 10 | ev3-uart: 11 | ev3-analog: 12 | 13 | #Driver names 14 | ev3_driver_name: 15 | lego-ev3-touch: 16 | lego-ev3-color: 17 | lego-ev3-us: 18 | lego-ev3-gyro: 19 | lego-ev3-ir: 20 | 21 | #Gyro modess 22 | ev3_gyro_mode: 23 | GYRO-ANG: 24 | GYRO-RATE: 25 | GYRO-FAS: 26 | GYRO-G&A: GYRO_G_A 27 | GYRO-CAL: 28 | 29 | #Ultrasonic modes 30 | ev3_ultrasonic_mode: 31 | US-DIST-CM: 32 | US-DIST-IN: 33 | US-LISTEN: 34 | US-SI-CM: 35 | US-SI-IN: 36 | US-DC-CM: 37 | US-DC-IN: 38 | 39 | #Color modes 40 | ev3_color_mode: 41 | COL-REFLECT: 42 | COL-AMBIENT: 43 | COL-COLOR: 44 | REF-RAW: 45 | RGB-RAW: 46 | COL-CAL: 47 | 48 | ev3_touch_mode: 49 | TOUCH: 50 | 51 | ev3_infrared_mode: 52 | IR-PROX: 53 | IR-SEEK: 54 | IR-REMOTE: 55 | IR-REM-A: 56 | IR-S-ALT: 57 | IR-CAL: 58 | 59 | 60 | ev3_motor_commands: 61 | run-forever: 62 | run-to-abs-pos: 63 | run-to-rel-pos: 64 | run-timed: 65 | run-direct: 66 | stop: 67 | reset: 68 | 69 | ev3_switch: 70 | 'on': #TODO: Why do i need to set that in '', WTF ? 71 | 'off': 72 | 73 | ev3_polarity: 74 | normal: 75 | inversed: 76 | 77 | 78 | -------------------------------------------------------------------------------- /h4r_ev3_launch/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(h4r_ev3_launch) 3 | find_package(catkin REQUIRED) 4 | catkin_package() 5 | 6 | install( 7 | DIRECTORY config launch 8 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 9 | ) 10 | -------------------------------------------------------------------------------- /h4r_ev3_launch/config/color.yaml: -------------------------------------------------------------------------------- 1 | Ev3Color: 2 | type: h4r_ev3_control/Ev3ColorController 3 | frame_id: color_link 4 | topic_name: /color 5 | publish_rate: 10 6 | port: in3 7 | mode: rgb_raw 8 | 9 | #Mode: 10 | # rgb_raw 11 | # color 12 | #0 none 13 | #1 black 14 | #2 blue 15 | #3 green 16 | #4 yellow 17 | #5 red 18 | #6 white 19 | #7 brown 20 | # ambient 21 | # reflect -------------------------------------------------------------------------------- /h4r_ev3_launch/config/gyro.yaml: -------------------------------------------------------------------------------- 1 | Ev3Gyro: 2 | type: h4r_ev3_control/Ev3GyroController #Controller Type 3 | frame_id: gyro_link #Frame ID for IMU message 4 | topic_name: /gyro #Topic Name 5 | publish_rate: 10 #Publish Rate 6 | port: in4 #EV3 Input Port 7 | mode: angle 8 | # mode: 9 | # angle - returns the angle in a imu message 10 | # rate - returns the turning rate 11 | # rate&angle - returns the angle and the rate (WARNING: according to EV3Dev documentation the driver crashes when reaching an angle of around 32000!)" 12 | -------------------------------------------------------------------------------- /h4r_ev3_launch/config/infrared.yaml: -------------------------------------------------------------------------------- 1 | Ev3Infrared: 2 | type: h4r_ev3_control/Ev3InfraredController 3 | frame_id: ir_link 4 | topic_name: /ir 5 | publish_rate: 10 6 | port: in4 7 | mode: remote 8 | -------------------------------------------------------------------------------- /h4r_ev3_launch/config/motors.yaml: -------------------------------------------------------------------------------- 1 | Ev3devJoints: 2 | Joint_A: 3 | speed_pid: [1001, 61 , 1] 4 | mode: velocity 5 | Joint_B: 6 | speed_pid: [1002, 62, 2] 7 | mode: velocity 8 | Joint_C: 9 | speed_pid: [1003 , 63, 3] 10 | mode: velocity 11 | Joint_D: 12 | speed_pid: [1004 , 64, 4] 13 | mode: velocity 14 | 15 | # Publish all joint states ----------------------------------- 16 | OutPortState: 17 | type: joint_state_controller/JointStateController 18 | publish_rate: 10 19 | 20 | # Joint velocity controller 21 | OutPortA: 22 | type: velocity_controllers/JointVelocityController 23 | joint: Joint_A 24 | 25 | OutPortB: 26 | type: velocity_controllers/JointVelocityController 27 | joint: Joint_B 28 | 29 | #### 30 | diffDrv: 31 | type : "diff_drive_controller/DiffDriveController" 32 | left_wheel : 'Joint_C' 33 | right_wheel : 'Joint_D' 34 | publish_rate: 10.0 # default: 50 35 | pose_covariance_diagonal : [0.000, 0.000, 0.0, 0.0, 0.0, 0.0] 36 | twist_covariance_diagonal: [0.000, 0.000, 0.0, 0.0, 0.0, 0.0] 37 | 38 | # Wheel separation and diameter. These are both optional. 39 | # diff_drive_controller will attempt to read either one or both from the 40 | # URDF if not specified as a parameter 41 | wheel_separation : 1.0 42 | wheel_radius : 0.15 43 | 44 | # Wheel separation and radius multipliers 45 | wheel_separation_multiplier: 1.0 # default: 1.0 46 | wheel_radius_multiplier : 1.0 # default: 1.0 47 | 48 | # Velocity commands timeout [s], default 0.5 49 | cmd_vel_timeout: 0.5 50 | 51 | # Base frame_id 52 | base_frame_id: base_footprint #default: base_link 53 | 54 | # Velocity and acceleration limits 55 | # Whenever a min_* is unspecified, default to -max_* 56 | linear: 57 | x: 58 | has_velocity_limits : true 59 | max_velocity : 1.0 # m/s 60 | min_velocity : -1.0 # m/s 61 | has_acceleration_limits: true 62 | max_acceleration : 0.8 # m/s^2 63 | min_acceleration : 0.2# m/s^2 64 | angular: 65 | z: 66 | has_velocity_limits : true 67 | max_velocity : 1.7 # rad/s 68 | has_acceleration_limits: true 69 | max_acceleration : 1.5 # rad/s^2 -------------------------------------------------------------------------------- /h4r_ev3_launch/config/touch.yaml: -------------------------------------------------------------------------------- 1 | Ev3Touch: 2 | type: h4r_ev3_control/Ev3TouchController 3 | frame_id: touch_link 4 | topic_name: /touch 5 | publish_rate: 10 6 | port: in1 7 | -------------------------------------------------------------------------------- /h4r_ev3_launch/config/ultrasonic.yaml: -------------------------------------------------------------------------------- 1 | Ev3UltraSonic: 2 | type: h4r_ev3_control/Ev3UltrasonicController 3 | max_range: 1.5 4 | min_range: 0.01 5 | frame_id: ultrasonic_link 6 | topic_name: /ultrasonic 7 | publish_rate: 10 8 | port: in2 9 | mode: distance -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/color.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/gyro.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/infrared.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/motors.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | ["outA","outB","outC","outD"] 5 | ["in1","in2","in3","in4"] 6 | 7 | 8 | 9 | 10 | 11 | 13 | 14 | 15 | 16 | -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/touch.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /h4r_ev3_launch/launch/ultrasonic.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 9 | 10 | 11 | -------------------------------------------------------------------------------- /h4r_ev3_launch/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | h4r_ev3_launch 4 | 0.0.0 5 | The h4r_ev3_launch package contains example launch files and configurations for h4r_ev3_control 6 | 7 | Christian Holl 8 | 9 | GPLv3 10 | 11 | http://hacks4ros.github.io/h4r_ev3_ctrl 12 | http://github.com/Hacks4ROS/h4r_ev3_ctrl 13 | https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues 14 | 15 | Christian Holl 16 | 17 | catkin 18 | controller_interface 19 | hardware_interface 20 | pluginlib 21 | 22 | controller_interface 23 | hardware_interface 24 | pluginlib 25 | 26 | 27 | 28 | -------------------------------------------------------------------------------- /h4r_ev3_manager/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(h4r_ev3_manager) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED COMPONENTS 8 | roscpp 9 | roslib 10 | diagnostic_msgs 11 | controller_manager 12 | hardware_interface 13 | joint_limits_interface 14 | control_toolbox 15 | h4r_ev3_control 16 | ) 17 | 18 | 19 | ## System dependencies are found with CMake's conventions 20 | find_package(Boost REQUIRED COMPONENTS system thread program_options ) 21 | 22 | ## Uncomment this if the package has a setup.py. This macro ensures 23 | ## modules and global scripts declared therein get installed 24 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 25 | # catkin_python_setup() 26 | 27 | ################################################ 28 | ## Declare ROS messages, services and actions ## 29 | ################################################ 30 | 31 | ## To declare and build messages, services or actions from within this 32 | ## package, follow these steps: 33 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 34 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 35 | ## * In the file package.xml: 36 | ## * add a build_depend tag for "message_generation" 37 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 38 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 39 | ## but can be declared for certainty nonetheless: 40 | ## * add a run_depend tag for "message_runtime" 41 | ## * In this file (CMakeLists.txt): 42 | ## * add "message_generation" and every package in MSG_DEP_SET to 43 | ## find_package(catkin REQUIRED COMPONENTS ...) 44 | ## * add "message_runtime" and every package in MSG_DEP_SET to 45 | ## catkin_package(CATKIN_DEPENDS ...) 46 | ## * uncomment the add_*_files sections below as needed 47 | ## and list every .msg/.srv/.action file to be processed 48 | ## * uncomment the generate_messages entry below 49 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 50 | 51 | ## Generate messages in the 'msg' folder 52 | #add_message_files( 53 | # FILES 54 | # 55 | #) 56 | 57 | ## Generate services in the 'srv' folder 58 | # add_service_files( 59 | # FILES 60 | # Service1.srv 61 | # Service2.srv 62 | # ) 63 | 64 | ## Generate actions in the 'action' folder 65 | # add_action_files( 66 | # FILES 67 | # Action1.action 68 | # Action2.action 69 | # ) 70 | 71 | ## Generate added messages and services with any dependencies listed here 72 | #generate_messages( 73 | # DEPENDENCIES 74 | # std_msgs # Or other packages containing msgs 75 | #) 76 | 77 | ################################################ 78 | ## Declare ROS dynamic reconfigure parameters ## 79 | ################################################ 80 | 81 | ## To declare and build dynamic reconfigure parameters within this 82 | ## package, follow these steps: 83 | ## * In the file package.xml: 84 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 85 | ## * In this file (CMakeLists.txt): 86 | ## * add "dynamic_reconfigure" to 87 | ## find_package(catkin REQUIRED COMPONENTS ...) 88 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 89 | ## and list every .cfg file to be processed 90 | 91 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 92 | # generate_dynamic_reconfigure_options( 93 | # cfg/DynReconf1.cfg 94 | # cfg/DynReconf2.cfg 95 | # ) 96 | 97 | ################################### 98 | ## catkin specific configuration ## 99 | ################################### 100 | ## The catkin_package macro generates cmake config files for your package 101 | ## Declare things to be passed to dependent projects 102 | ## INCLUDE_DIRS: uncomment this if you package contains header files 103 | ## LIBRARIES: libraries you create in this project that dependent projects also need 104 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 105 | ## DEPENDS: system dependencies of this project that dependent projects also need 106 | catkin_package( 107 | INCLUDE_DIRS include 108 | # LIBRARIES h4r_ev3_ctrl 109 | CATKIN_DEPENDS roscpp 110 | # roslib 111 | # message_runtime 112 | # DEPENDS system_lib 113 | ) 114 | 115 | ########### 116 | ## Build ## 117 | ########### 118 | 119 | ## Specify additional locations of header files 120 | ## Your package locations should be listed before other locations 121 | # include_directories(include) 122 | include_directories( 123 | include 124 | ${catkin_INCLUDE_DIRS} 125 | ) 126 | 127 | ## Declare a C++ library 128 | 129 | 130 | ## Add cmake target dependencies of the library 131 | ## as an example, code may need to be generated before libraries 132 | ## either from message generation or dynamic reconfigure 133 | # add_dependencies(h4r_ev3_ctrl ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 134 | 135 | #C++ 11 136 | add_definitions(-std=c++11) 137 | 138 | ##################### 139 | # H4R EV3 CTRL NODE # 140 | ##################### 141 | add_executable(ev3_manager_node 142 | src/${PROJECT_NAME}/ev3_ctrl_node.cpp 143 | src/${PROJECT_NAME}/Ev3HardwareInterface.cpp 144 | ) 145 | 146 | target_link_libraries(ev3_manager_node 147 | ${catkin_LIBRARIES} 148 | ) 149 | 150 | ######################### 151 | 152 | 153 | ## Add cmake target dependencies of the executable 154 | ## same as for the library above 155 | 156 | 157 | ## Specify libraries to link a library or executable target against 158 | 159 | 160 | ############# 161 | ## Install ## 162 | ############# 163 | 164 | # all install targets should use catkin DESTINATION variables 165 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 166 | 167 | ## Mark executable scripts (Python etc.) for installation 168 | ## in contrast to setup.py, you can choose the destination 169 | # install(PROGRAMS 170 | # scripts/my_python_script 171 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 172 | # ) 173 | 174 | ## Mark executables and/or libraries for installation 175 | install(TARGETS ev3_manager_node 176 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 177 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 178 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 179 | ) 180 | 181 | ##Mark cpp header files for installation 182 | install(DIRECTORY include/${PROJECT_NAME}/ 183 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 184 | FILES_MATCHING PATTERN "*.h" 185 | # PATTERN ".svn" EXCLUDE 186 | ) 187 | 188 | ## Install launch files 189 | install(DIRECTORY launch 190 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 191 | FILES_MATCHING PATTERN "*.launch" 192 | ) 193 | 194 | ## Mark other files for installation (e.g. launch and bag files, etc.) 195 | install(FILES 196 | launch/ev3.launch 197 | ctrl_cfg/ev3.yaml 198 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 199 | ) 200 | 201 | 202 | ############# 203 | ## Testing ## 204 | ############# 205 | 206 | ## Add gtest based cpp test target and link libraries 207 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_h4r_ev3_ctrl.cpp) 208 | # if(TARGET ${PROJECT_NAME}-test) 209 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 210 | # endif() 211 | 212 | ## Add folders to be run by python nosetests 213 | # catkin_add_nosetests(test) 214 | -------------------------------------------------------------------------------- /h4r_ev3_manager/ctrl_cfg/motors.yaml: -------------------------------------------------------------------------------- 1 | Ev3devJoints: 2 | Joint_A: 3 | speed_pid: [1001, 61 , 1] 4 | mode: velocity 5 | Joint_B: 6 | speed_pid: [1002, 62, 2] 7 | mode: velocity 8 | Joint_C: 9 | speed_pid: [1003 , 63, 3] 10 | mode: velocity 11 | Joint_D: 12 | speed_pid: [1004 , 64, 4] 13 | mode: velocity 14 | 15 | # Publish all joint states ----------------------------------- 16 | OutPortState: 17 | type: joint_state_controller/JointStateController 18 | publish_rate: 10 19 | 20 | # Joint velocity controller 21 | OutPortA: 22 | type: velocity_controllers/JointVelocityController 23 | joint: Joint_A 24 | 25 | OutPortB: 26 | type: velocity_controllers/JointVelocityController 27 | joint: Joint_B 28 | 29 | #### 30 | diffDrv: 31 | type : "diff_drive_controller/DiffDriveController" 32 | left_wheel : 'Joint_C' 33 | right_wheel : 'Joint_D' 34 | publish_rate: 10.0 # default: 50 35 | pose_covariance_diagonal : [0.000, 0.000, 0.0, 0.0, 0.0, 0.0] 36 | twist_covariance_diagonal: [0.000, 0.000, 0.0, 0.0, 0.0, 0.0] 37 | 38 | # Wheel separation and diameter. These are both optional. 39 | # diff_drive_controller will attempt to read either one or both from the 40 | # URDF if not specified as a parameter 41 | wheel_separation : 1.0 42 | wheel_radius : 0.15 43 | 44 | # Wheel separation and radius multipliers 45 | wheel_separation_multiplier: 1.0 # default: 1.0 46 | wheel_radius_multiplier : 1.0 # default: 1.0 47 | 48 | # Velocity commands timeout [s], default 0.5 49 | cmd_vel_timeout: 0.5 50 | 51 | # Base frame_id 52 | base_frame_id: base_footprint #default: base_link 53 | 54 | # Velocity and acceleration limits 55 | # Whenever a min_* is unspecified, default to -max_* 56 | linear: 57 | x: 58 | has_velocity_limits : true 59 | max_velocity : 1.0 # m/s 60 | min_velocity : -1.0 # m/s 61 | has_acceleration_limits: true 62 | max_acceleration : 0.8 # m/s^2 63 | min_acceleration : 0.2# m/s^2 64 | angular: 65 | z: 66 | has_velocity_limits : true 67 | max_velocity : 1.7 # rad/s 68 | has_acceleration_limits: true 69 | max_acceleration : 1.5 # rad/s^2 -------------------------------------------------------------------------------- /h4r_ev3_manager/include/h4r_ev3_joint_setup/ev3_joint_settings.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_joint_settings.h) is part of h4r_ev3_manager. 3 | * Date: 16.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_joint_setup. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef EV3_JOINT_SETTINGS_H_ 24 | #define EV3_JOINT_SETTINGS_H_ 25 | 26 | #include 27 | 28 | namespace ev3_control { 29 | class Ev3JointSettings 30 | { 31 | 32 | public: 33 | 34 | typedef enum 35 | { 36 | EV3_JOINT_OFF, 37 | EV3_JOINT_POSITION, 38 | EV3_JOINT_VELOCITY, 39 | } Ev3JointMode; 40 | 41 | class Ev3HwSettings 42 | { 43 | public: 44 | Ev3HwSettings() 45 | :joint_mode(EV3_JOINT_VELOCITY) 46 | {} 47 | 48 | Ev3JointMode joint_mode; 49 | std::string driver_name; 50 | std::vector pid; 51 | }; 52 | 53 | 54 | H4REv3Motor port; 55 | Ev3HwSettings ev3settings; 56 | 57 | double command; 58 | double last_command; 59 | double velocity_out; 60 | double position_out; 61 | double effort_out; //not supported by ev3 afaik but needed for joint state handle 62 | 63 | Ev3JointSettings(const std::string &out_port) 64 | :port(out_port) 65 | ,command(0) 66 | ,last_command(0) 67 | ,velocity_out(0) 68 | ,position_out(0) 69 | ,effort_out(0) 70 | {} 71 | 72 | 73 | bool load(const Ev3HwSettings &settings, bool testOnly) 74 | { 75 | 76 | 77 | if(!testOnly) 78 | { 79 | port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_RESET); 80 | } 81 | 82 | 83 | ROS_INFO_STREAM("PID "<0) 117 | { 118 | if(!port.setMotorPolarity(Ev3Strings::EV3POLARITY_NORMAL)) 119 | { 120 | ROS_ERROR("NORMAL!"); 121 | return false; 122 | } 123 | } 124 | else if(cmd<0) 125 | { 126 | cmd=-cmd; 127 | if(!port.setMotorPolarity(Ev3Strings::EV3POLARITY_INVERSED)) 128 | { 129 | ROS_ERROR("INVERSED!"); 130 | return false; 131 | } 132 | } 133 | switch(ev3settings.joint_mode) 134 | { 135 | 136 | case Ev3JointSettings::EV3_JOINT_POSITION: 137 | 138 | if 139 | ( 140 | port.setDutyCycleSP(100)+ 141 | port.setPositionSP(cmd)+ 142 | port.setSpeedRegulation(Ev3Strings::EV3SWITCH_OFF)+ 143 | port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_RUN_TO_ABS_POS) 144 | !=4 145 | ) 146 | { 147 | return false; 148 | } 149 | 150 | break; 151 | 152 | case Ev3JointSettings::EV3_JOINT_VELOCITY: 153 | { 154 | if(cmd==0) 155 | { 156 | return port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_STOP); 157 | } 158 | else 159 | { 160 | 161 | if 162 | ( 163 | port.setDutyCycleSP(100)+ 164 | port.setSpeedSP(cmd)+ 165 | port.setSpeedRegulation(Ev3Strings::EV3SWITCH_ON)+ 166 | port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_RUN_FOREVER) 167 | != 4 168 | ) 169 | return false; 170 | } 171 | } 172 | break; 173 | 174 | default: 175 | break; 176 | } 177 | 178 | return true; 179 | } 180 | 181 | bool read() 182 | { 183 | if(!port.isConnected()) 184 | return false; 185 | 186 | int pos; 187 | int vel; 188 | 189 | if( 190 | port.position(pos)+port.speed(vel) == 2 191 | ) 192 | { 193 | velocity_out=((double)vel)/180.0*M_PI; 194 | position_out=((double)pos)/180.0*M_PI; 195 | return true; 196 | } 197 | else 198 | { 199 | return false; 200 | } 201 | } 202 | 203 | }; 204 | 205 | } 206 | 207 | #endif /* EV3_JOINT_SETTINGS_H_ */ 208 | -------------------------------------------------------------------------------- /h4r_ev3_manager/include/h4r_ev3_joint_setup/ev3_joint_settings_exception.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_joint_settings_exception.h) is part of h4r_ev3_manager. 3 | * Date: 17.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_joint_setup. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef EV3_JOINT_SETTINGS_EXCEPTION_H_ 24 | #define EV3_JOINT_SETTINGS_EXCEPTION_H_ 25 | 26 | #include 27 | #include 28 | 29 | 30 | namespace ev3_control 31 | { 32 | 33 | class Ev3JointInterfaceException: public std::exception 34 | { 35 | public: 36 | Ev3JointInterfaceException(const std::string& msg) 37 | : msg(msg) {} 38 | 39 | virtual ~Ev3JointInterfaceException() throw() 40 | {} 41 | 42 | virtual const char* what() const throw() 43 | { 44 | return msg.c_str(); 45 | } 46 | 47 | private: 48 | std::string msg; 49 | }; 50 | 51 | } 52 | 53 | #endif /* EV3_JOINT_SETTINGS_EXCEPTION_H_ */ 54 | -------------------------------------------------------------------------------- /h4r_ev3_manager/include/h4r_ev3_joint_setup/ev3_joint_settings_interface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_joint_settings_interface.h) is part of h4r_ev3_manager. 3 | * Date: 19.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_joint_setting. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef EV3_JOINT_SETTINGS_INTERFACE_H 24 | #define EV3_JOINT_SETTINGS_INTERFACE_H 25 | 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | 40 | 41 | 42 | namespace ev3_control 43 | { 44 | using namespace hardware_interface; 45 | using namespace std; 46 | 47 | 48 | class Ev3JointInterfaceHandle 49 | { 50 | public: 51 | 52 | 53 | 54 | Ev3JointInterfaceHandle() 55 | :settings_(0){} 56 | 57 | Ev3JointInterfaceHandle( 58 | const JointHandle& jh, 59 | Ev3JointSettings *settings) 60 | :jh_(jh) 61 | ,settings_(settings) 62 | {} 63 | 64 | std::string getName() const 65 | { 66 | return jh_.getName(); 67 | } 68 | 69 | Ev3JointSettings &getSettings() 70 | { 71 | return *settings_; 72 | } 73 | 74 | 75 | private: 76 | 77 | 78 | Ev3JointSettings *settings_; 79 | JointHandle jh_; 80 | }; 81 | 82 | class Ev3JointInterface : public ResourceManager 83 | { 84 | public: 85 | Ev3JointInterfaceHandle getHandle(const std::string& name) 86 | { 87 | // Rethrow exception with a meaningful type 88 | try 89 | { 90 | return this->ResourceManager::getHandle(name); 91 | } 92 | catch(const std::logic_error& e) 93 | { 94 | throw Ev3JointInterfaceException(e.what()); 95 | } 96 | } 97 | 98 | bool ControllerTestChange(const std::list &start_list) const 99 | { 100 | return true; 101 | } 102 | 103 | 104 | bool ControllerChange(const std::list &start_list) 105 | { 106 | ROS_INFO("Controller Change"); 107 | for (std::list::const_iterator it = start_list.begin(); it != start_list.end(); it++) 108 | { 109 | for (std::set::const_iterator res = it->resources.begin(); res != it->resources.end(); res++) 110 | { 111 | ROS_INFO_STREAM(it->type<<" requests Joint "<name<<" "<<*res); 112 | 113 | try 114 | { 115 | Ev3JointInterfaceHandle handle=getHandle(*res); 116 | Ev3JointSettings::Ev3HwSettings ev3settings; 117 | 118 | handle.getSettings().port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_RESET); 119 | getJointSettings(*res, ev3settings); 120 | ROS_INFO("----------------------------------------------<"); 121 | handle.getSettings().load(ev3settings,true); 122 | } 123 | catch(const Ev3JointInterfaceException& e) 124 | { 125 | ROS_ERROR_STREAM("Did not find handle: "<<*res); 126 | } 127 | } 128 | } 129 | return true; 130 | } 131 | 132 | 133 | }; 134 | 135 | 136 | 137 | } 138 | 139 | #endif /* EV3_JOINT_SETTINGS_INTERFACE_H */ 140 | -------------------------------------------------------------------------------- /h4r_ev3_manager/include/h4r_ev3_joint_setup/ev3_joint_settings_params.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_joint_params.h) is part of h4r_ev3_manager. 3 | * Date: 16.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_outport_setup. If not, see . 20 | * 21 | */ 22 | 23 | #ifndef EV3_JOINT_SETTINGS_PARAMS_H_ 24 | #define EV3_JOINT_SETTINGS_PARAMS_H_ 25 | 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | 32 | namespace ev3_control 33 | { 34 | 35 | inline bool getJointSettings(const std::string& joint_name, 36 | Ev3JointSettings::Ev3HwSettings& settings) 37 | { 38 | 39 | 40 | 41 | 42 | ros::NodeHandle setup_nh; 43 | const std::string joint_setup = "Ev3devJoints/"+joint_name; 44 | try 45 | { 46 | if (!setup_nh.hasParam(joint_setup)) 47 | { 48 | ROS_WARN_STREAM("No ev3 port settings found for ev3 joint '" << joint_name <<" using velocity mode and automatic port mode"); 49 | return false; 50 | } 51 | setup_nh = ros::NodeHandle(setup_nh, joint_setup); 52 | } 53 | catch (const ros::InvalidNameException& e) 54 | { 55 | ROS_ERROR_STREAM(e.what()); 56 | return false; 57 | } 58 | 59 | ROS_INFO_STREAM("<--------------EV3 Joint "<"); 60 | 61 | 62 | std::string mode; 63 | if(setup_nh.getParam("mode", mode)) 64 | { 65 | if(mode=="position") 66 | { 67 | settings.joint_mode=Ev3JointSettings::EV3_JOINT_POSITION; 68 | } 69 | else if(mode=="velocity") 70 | { 71 | settings.joint_mode=Ev3JointSettings::EV3_JOINT_VELOCITY; 72 | } 73 | else 74 | { 75 | ROS_WARN_STREAM("Unknown Mode for Ev3 Joint: "<"); 115 | 116 | return true; 117 | } 118 | } 119 | 120 | 121 | #endif /* EV3_JOINT_SETTINGS_PARAMS_H_ */ 122 | -------------------------------------------------------------------------------- /h4r_ev3_manager/launch/load.launch: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | 5 | 6 | 7 | 8 | 10 | 11 | 12 | -------------------------------------------------------------------------------- /h4r_ev3_manager/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | h4r_ev3_manager 4 | 0.0.0 5 | The h4r_ev3_manager package 6 | Christian Holl 7 | GPLv3 8 | 9 | 10 | http://hacks4ros.github.io/h4r_ev3_ctrl 11 | http://github.com/Hacks4ROS/h4r_ev3_ctrl 12 | https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues 13 | 14 | Christian Holl 15 | 16 | catkin 17 | 18 | roscpp 19 | roscpp 20 | 21 | joint_limits_interface 22 | joint_limits_interface 23 | 24 | control_toolbox 25 | control_toolbox 26 | 27 | roslib 28 | roslib 29 | 30 | diagnostic_msgs 31 | diagnostic_msgs 32 | 33 | controller_manager 34 | controller_manager 35 | 36 | hardware_interface 37 | hardware_interface 38 | 39 | 40 | h4r_ev3_control 41 | h4r_ev3_control 42 | 43 | 44 | 45 | 46 | -------------------------------------------------------------------------------- /h4r_ev3_manager/src/h4r_ev3_manager/Ev3HardwareInterface.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (Ev3HardwareInterface.cpp) is part of h4r_ev3_manager. 3 | * Date: 16.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #include "Ev3HardwareInterface.h" 24 | 25 | 26 | namespace ev3_control { 27 | Ev3HardwareInterface::Ev3HardwareInterface( 28 | const std::vector &in_ports, 29 | const std::vector &out_ports 30 | ):nh_(0) 31 | { 32 | registerInterface(&jnt_vel_interface); 33 | registerInterface(&jnt_pos_interface); 34 | registerInterface(&jnt_state_interface); 35 | registerInterface(&sensor_interface); 36 | 37 | for(int p=0; p < in_ports.size(); ++p) 38 | { 39 | ev3_sensors_.push_back(new H4REv3Sensor(in_ports[p])); 40 | ev3_control::Ev3SensorHandle handle(in_ports[p],ev3_sensors_[p]); 41 | sensor_interface.registerHandle(handle); 42 | } 43 | 44 | for (int p = 0; p < out_ports.size(); ++p) 45 | { 46 | //int p=0; 47 | //Create new out port data - pointer because of vector reallocations... 48 | joint_settings_.push_back(new Ev3JointSettings(out_ports[p])); 49 | 50 | //Create jointname with letter 51 | string joint_name; 52 | joint_name+="Joint_"; 53 | joint_name+='A'+p; 54 | 55 | //Register motor joints 56 | hardware_interface::JointStateHandle state_handle(joint_name, &joint_settings_[p]->position_out, &joint_settings_[p]->velocity_out, &joint_settings_[p]->effort_out); 57 | jnt_state_interface.registerHandle(state_handle); 58 | 59 | hardware_interface::JointHandle joint_handle(jnt_state_interface.getHandle(joint_name), &joint_settings_[p]->command); 60 | jnt_vel_interface.registerHandle(joint_handle); 61 | jnt_pos_interface.registerHandle(joint_handle); 62 | 63 | //Joint limits 64 | joint_limits_interface::JointLimits limits; 65 | limits.has_velocity_limits=true; 66 | limits.max_velocity=0.1; 67 | 68 | joint_limits_interface::SoftJointLimits soft_limits; 69 | joint_limits_interface::PositionJointSoftLimitsHandle pos_limit_handle(jnt_pos_interface.getHandle(joint_name), limits, soft_limits); 70 | jnt_limits_interface.registerHandle(pos_limit_handle); 71 | 72 | Ev3JointInterfaceHandle ev3_joint_handle(joint_handle,joint_settings_[p]); 73 | jnt_ev3_joint_interface.registerHandle(ev3_joint_handle); 74 | 75 | } 76 | } 77 | 78 | 79 | Ev3HardwareInterface::~Ev3HardwareInterface() 80 | { 81 | 82 | for (int i = 0; i < joint_settings_.size(); ++i) 83 | { 84 | //reset motors so running motors will be stopped... 85 | joint_settings_[i]->port.setMotorCommand(Ev3Strings::EV3MOTORCOMMANDS_RESET); 86 | 87 | delete joint_settings_[i]; 88 | } 89 | joint_settings_.clear(); 90 | 91 | 92 | for(int i = 0; i < ev3_sensors_.size(); i++) 93 | { 94 | delete ev3_sensors_[i]; 95 | } 96 | ev3_sensors_.clear(); 97 | } 98 | 99 | void Ev3HardwareInterface::write(const ros::Duration &d) 100 | { 101 | // 102 | // 103 | // std::set claims=jnt_pos_interface.getClaims(); 104 | // 105 | // for (std::set::iterator it = claims.begin(); it != claims.end(); ++it) { 106 | // ROS_INFO_STREAM("P:::"<<*it); 107 | // } 108 | // claims.clear(); 109 | // 110 | // claims=jnt_vel_interface.getClaims(); 111 | // 112 | // for (std::set::iterator it = claims.begin(); it != claims.end(); ++it) { 113 | // ROS_INFO_STREAM("V:::"<<*it); 114 | // } 115 | // 116 | // claims.clear(); 117 | // claims=jnt_state_interface.getClaims(); 118 | // 119 | // for (std::set::iterator it = claims.begin(); it != claims.end(); ++it) 120 | // { 121 | // ROS_INFO_STREAM("S:::"<<*it); 122 | // } 123 | // 124 | 125 | 126 | //jnt_limits_interface.enforceLimits(d); 127 | for (int i = 0; i < joint_settings_.size(); ++i) 128 | { 129 | joint_settings_[i]->write(); 130 | 131 | 132 | } 133 | } 134 | 135 | 136 | 137 | void Ev3HardwareInterface::read() 138 | { 139 | 140 | for (int i = 0; i < joint_settings_.size(); ++i) 141 | { 142 | joint_settings_[i]->read(); 143 | } 144 | 145 | } 146 | 147 | 148 | bool Ev3HardwareInterface::canSwitch(const std::list &start_list, const std::list &stop_list) const 149 | { 150 | return jnt_ev3_joint_interface.ControllerTestChange(start_list); 151 | } 152 | 153 | void Ev3HardwareInterface::doSwitch(const std::list &start_list, const std::list &stop_list) 154 | { 155 | jnt_state_interface.getClaims(); 156 | jnt_ev3_joint_interface.ControllerChange(start_list); 157 | } 158 | 159 | } /* namespace ev3_control */ 160 | -------------------------------------------------------------------------------- /h4r_ev3_manager/src/h4r_ev3_manager/Ev3HardwareInterface.h: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (Ev3HardwareInterface.h) is part of h4r_ev3_manager. 3 | * Date: 19.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with h4r_ev3_joint_settings. If not, see . 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | 37 | #ifndef EV3HARDWAREINTERFACE_H_ 38 | #define EV3HARDWAREINTERFACE_H_ 39 | 40 | using namespace std; 41 | using namespace hardware_interface; 42 | 43 | namespace ev3_control { 44 | 45 | class Ev3HardwareInterface : public hardware_interface::RobotHW 46 | { 47 | ros::NodeHandle *nh_; 48 | 49 | std::vector joint_settings_; 50 | std::vector ev3_sensors_; 51 | 52 | 53 | hardware_interface::JointStateInterface jnt_state_interface; 54 | hardware_interface::VelocityJointInterface jnt_vel_interface; 55 | hardware_interface::PositionJointInterface jnt_pos_interface; 56 | hardware_interface::EffortJointInterface jnt_eff_interface; 57 | ev3_control::Ev3SensorInterface sensor_interface; 58 | 59 | joint_limits_interface::PositionJointSoftLimitsInterface jnt_limits_interface; 60 | Ev3JointInterface jnt_ev3_joint_interface; 61 | 62 | public: 63 | Ev3HardwareInterface( 64 | const std::vector &in_ports, 65 | const std::vector &out_ports 66 | ); 67 | virtual ~Ev3HardwareInterface(); 68 | 69 | void write(const ros::Duration &d); 70 | void read(); 71 | 72 | bool canSwitch(const std::list &start_list, const std::list &stop_list) const; 73 | void doSwitch(const std::list &start_list, const std::list &stop_list); 74 | }; 75 | 76 | } /* namespace ev3_control */ 77 | 78 | #endif /* EV3HARDWAREINTERFACE_H_ */ 79 | -------------------------------------------------------------------------------- /h4r_ev3_manager/src/h4r_ev3_manager/ev3_ctrl_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * This file (ev3_ctrl_node.cpp) is part of h4r_ev3_manager. 3 | * Date: 13.11.2015 4 | * 5 | * Author: Christian Holl 6 | * http://github.com/Hacks4ROS 7 | * 8 | * h4r_ev3_manager is free software: you can redistribute it and/or modify 9 | * it under the terms of the GNU General Public License as published by 10 | * the Free Software Foundation, either version 3 of the License, or 11 | * (at your option) any later version. 12 | * 13 | * h4r_ev3_manager is distributed in the hope that it will be useful, 14 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | * GNU General Public License for more details. 17 | * 18 | * You should have received a copy of the GNU General Public License 19 | * along with ev3_control. If not, see . 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include "Ev3HardwareInterface.h" 30 | using namespace std; 31 | 32 | int main(int argc, char** argv) 33 | { 34 | ros::init(argc,argv,"Ev3ControlNode"); 35 | ros::Time::init(); 36 | 37 | ros::Rate master_wait_rate(2); 38 | while(!ros::master::check()) 39 | { 40 | ROS_ERROR_ONCE("Waiting for rosmaster..."); 41 | if(!ros::ok()) 42 | { 43 | return 0; 44 | } 45 | master_wait_rate.sleep(); 46 | } 47 | 48 | 49 | ros::NodeHandle n; 50 | ros::NodeHandle nh("~"); 51 | 52 | ros::CallbackQueue queue; 53 | nh.setCallbackQueue(&queue); 54 | n.setCallbackQueue(&queue); 55 | 56 | ros::AsyncSpinner spinner(4, &queue); 57 | spinner.start(); 58 | 59 | 60 | std::vector out_ports, in_ports; 61 | nh.getParam("OutPorts",out_ports); 62 | nh.getParam("InPorts", in_ports); 63 | 64 | if(out_ports.size()==0) 65 | { 66 | out_ports.push_back("outA"); 67 | out_ports.push_back("outB"); 68 | out_ports.push_back("outC"); 69 | out_ports.push_back("outD"); 70 | } 71 | 72 | if(in_ports.size()==0) 73 | { 74 | in_ports.push_back("in1"); 75 | in_ports.push_back("in2"); 76 | in_ports.push_back("in3"); 77 | in_ports.push_back("in4"); 78 | } 79 | 80 | 81 | ev3_control::Ev3HardwareInterface robot(in_ports,out_ports); 82 | controller_manager::ControllerManager cm(&robot,n); 83 | 84 | ros::Time ts = ros::Time::now(); 85 | 86 | ros::Rate loop_rate(10); 87 | while (ros::ok()) 88 | { 89 | ros::Duration d = ros::Time::now() - ts; 90 | robot.read(); 91 | ts = ros::Time::now(); 92 | cm.update(ts, d); 93 | robot.write(d); 94 | loop_rate.sleep(); 95 | } 96 | 97 | spinner.stop(); 98 | 99 | return 0; 100 | } 101 | -------------------------------------------------------------------------------- /h4r_ev3_msgs/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(h4r_ev3_msgs) 3 | 4 | ## Find catkin macros and libraries 5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz) 6 | ## is used, also find other catkin packages 7 | find_package(catkin REQUIRED message_generation) 8 | 9 | ## System dependencies are found with CMake's conventions 10 | # find_package(Boost REQUIRED COMPONENTS system) 11 | 12 | 13 | ## Uncomment this if the package has a setup.py. This macro ensures 14 | ## modules and global scripts declared therein get installed 15 | ## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html 16 | # catkin_python_setup() 17 | 18 | ################################################ 19 | ## Declare ROS messages, services and actions ## 20 | ################################################ 21 | 22 | ## To declare and build messages, services or actions from within this 23 | ## package, follow these steps: 24 | ## * Let MSG_DEP_SET be the set of packages whose message types you use in 25 | ## your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...). 26 | ## * In the file package.xml: 27 | ## * add a build_depend tag for "message_generation" 28 | ## * add a build_depend and a run_depend tag for each package in MSG_DEP_SET 29 | ## * If MSG_DEP_SET isn't empty the following dependency has been pulled in 30 | ## but can be declared for certainty nonetheless: 31 | ## * add a run_depend tag for "message_runtime" 32 | ## * In this file (CMakeLists.txt): 33 | ## * add "message_generation" and every package in MSG_DEP_SET to 34 | ## find_package(catkin REQUIRED COMPONENTS ...) 35 | ## * add "message_runtime" and every package in MSG_DEP_SET to 36 | ## catkin_package(CATKIN_DEPENDS ...) 37 | ## * uncomment the add_*_files sections below as needed 38 | ## and list every .msg/.srv/.action file to be processed 39 | ## * uncomment the generate_messages entry below 40 | ## * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...) 41 | 42 | ## Generate messages in the 'msg' folder 43 | add_message_files( 44 | FILES 45 | Seek.msg 46 | ) 47 | 48 | ## Generate services in the 'srv' folder 49 | # add_service_files( 50 | # FILES 51 | # Service1.srv 52 | # Service2.srv 53 | # ) 54 | 55 | ## Generate actions in the 'action' folder 56 | # add_action_files( 57 | # FILES 58 | # Action1.action 59 | # Action2.action 60 | # ) 61 | 62 | ## Generate added messages and services with any dependencies listed here 63 | generate_messages( 64 | # DEPENDENCIES 65 | # std_msgs # Or other packages containing msgs 66 | ) 67 | 68 | ################################################ 69 | ## Declare ROS dynamic reconfigure parameters ## 70 | ################################################ 71 | 72 | ## To declare and build dynamic reconfigure parameters within this 73 | ## package, follow these steps: 74 | ## * In the file package.xml: 75 | ## * add a build_depend and a run_depend tag for "dynamic_reconfigure" 76 | ## * In this file (CMakeLists.txt): 77 | ## * add "dynamic_reconfigure" to 78 | ## find_package(catkin REQUIRED COMPONENTS ...) 79 | ## * uncomment the "generate_dynamic_reconfigure_options" section below 80 | ## and list every .cfg file to be processed 81 | 82 | ## Generate dynamic reconfigure parameters in the 'cfg' folder 83 | # generate_dynamic_reconfigure_options( 84 | # cfg/DynReconf1.cfg 85 | # cfg/DynReconf2.cfg 86 | # ) 87 | 88 | ################################### 89 | ## catkin specific configuration ## 90 | ################################### 91 | ## The catkin_package macro generates cmake config files for your package 92 | ## Declare things to be passed to dependent projects 93 | ## INCLUDE_DIRS: uncomment this if you package contains header files 94 | ## LIBRARIES: libraries you create in this project that dependent projects also need 95 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 96 | ## DEPENDS: system dependencies of this project that dependent projects also need 97 | catkin_package( 98 | # INCLUDE_DIRS include 99 | # LIBRARIES h4r_ev3_msgs 100 | CATKIN_DEPENDS message_runtime 101 | # DEPENDS system_lib 102 | ) 103 | 104 | ########### 105 | ## Build ## 106 | ########### 107 | 108 | ## Specify additional locations of header files 109 | ## Your package locations should be listed before other locations 110 | # include_directories(include) 111 | 112 | ## Declare a C++ library 113 | # add_library(h4r_ev3_msgs 114 | # src/${PROJECT_NAME}/h4r_ev3_msgs.cpp 115 | # ) 116 | 117 | ## Add cmake target dependencies of the library 118 | ## as an example, code may need to be generated before libraries 119 | ## either from message generation or dynamic reconfigure 120 | # add_dependencies(h4r_ev3_msgs ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 121 | 122 | ## Declare a C++ executable 123 | # add_executable(h4r_ev3_msgs_node src/h4r_ev3_msgs_node.cpp) 124 | 125 | ## Add cmake target dependencies of the executable 126 | ## same as for the library above 127 | # add_dependencies(h4r_ev3_msgs_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS}) 128 | 129 | ## Specify libraries to link a library or executable target against 130 | # target_link_libraries(h4r_ev3_msgs_node 131 | # ${catkin_LIBRARIES} 132 | # ) 133 | 134 | ############# 135 | ## Install ## 136 | ############# 137 | 138 | # all install targets should use catkin DESTINATION variables 139 | # See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html 140 | 141 | ## Mark executable scripts (Python etc.) for installation 142 | ## in contrast to setup.py, you can choose the destination 143 | # install(PROGRAMS 144 | # scripts/my_python_script 145 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 146 | # ) 147 | 148 | ## Mark executables and/or libraries for installation 149 | # install(TARGETS h4r_ev3_msgs h4r_ev3_msgs_node 150 | # ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 151 | # LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 152 | # RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 153 | # ) 154 | 155 | ## Mark cpp header files for installation 156 | # install(DIRECTORY include/${PROJECT_NAME}/ 157 | # DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 158 | # FILES_MATCHING PATTERN "*.h" 159 | # PATTERN ".svn" EXCLUDE 160 | # ) 161 | 162 | ## Mark other files for installation (e.g. launch and bag files, etc.) 163 | # install(FILES 164 | # # myfile1 165 | # # myfile2 166 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 167 | # ) 168 | 169 | ############# 170 | ## Testing ## 171 | ############# 172 | 173 | ## Add gtest based cpp test target and link libraries 174 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_h4r_ev3_msgs.cpp) 175 | # if(TARGET ${PROJECT_NAME}-test) 176 | # target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME}) 177 | # endif() 178 | 179 | ## Add folders to be run by python nosetests 180 | # catkin_add_nosetests(test) 181 | -------------------------------------------------------------------------------- /h4r_ev3_msgs/msg/Seek.msg: -------------------------------------------------------------------------------- 1 | float64 heading 2 | float64 distance -------------------------------------------------------------------------------- /h4r_ev3_msgs/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | h4r_ev3_msgs 4 | 0.0.0 5 | The h4r_ev3_msgs package 6 | 7 | 8 | Christian Holl 9 | Christian Holl 10 | GPLv3 11 | 12 | http://hacks4ros.github.io/h4r_ev3_ctrl 13 | http://github.com/Hacks4ROS/h4r_ev3_ctrl 14 | https://github.com/Hacks4ROS/h4r_ev3_ctrl/issues 15 | 16 | message_generation 17 | message_runtime 18 | catkin 19 | 20 | 21 | 22 | 23 | --------------------------------------------------------------------------------