diff --git a/.gitignore b/.gitignore new file mode 100644 index 0000000..378eac2 --- /dev/null +++ b/.gitignore @@ -0,0 +1 @@ +build diff --git a/CMakeLists.txt b/CMakeLists.txt new file mode 100644 index 0000000..7fcd117 --- /dev/null +++ b/CMakeLists.txt @@ -0,0 +1,68 @@ +cmake_minimum_required(VERSION 2.8 FATAL_ERROR) + +project(HokuyoAIST) +string(TOLOWER ${PROJECT_NAME} PROJECT_NAME_LOWER) +include(${PROJECT_SOURCE_DIR}/cmake/hokuyoaist_utils.cmake) +set(PROJECT_VERSION 3.0.0 CACHE STRING "HokuyoAIST version") +DISSECT_VERSION() +set(PROJECT_DESCRIPTION "Hokuyo range sensor driver.") +set(PROJECT_VENDOR "Geoffrey Biggs, AIST") + +# Add an "uninstall" target +configure_file("${PROJECT_SOURCE_DIR}/cmake/uninstall_target.cmake.in" + "${PROJECT_BINARY_DIR}/uninstall_target.cmake" IMMEDIATE @ONLY) +add_custom_target(uninstall "${CMAKE_COMMAND}" -P + "${PROJECT_BINARY_DIR}/uninstall_target.cmake") + +option(BUILD_EXAMPLES "Build and install examples" ON) +option(BUILD_PYTHON_BINDINGS "Build Python bindings" OFF) +option(BUILD_DOCUMENTATION "Build the documentation" ON) + +option(HOKUYOAIST_STATIC_LIBS "Build static libraries" OFF) +if(HOKUYOAIST_STATIC_LIBS) + set(LIB_TYPE STATIC) +else(HOKUYOAIST_STATIC_LIBS) + set(LIB_TYPE SHARED) +endif(HOKUYOAIST_STATIC_LIBS) + +# Set up installation directories +set(BIN_INSTALL_DIR "bin") +set(LIB_INSTALL_DIR "lib") +set(INC_INSTALL_DIR + "include/${PROJECT_NAME_LOWER}-${PROJECT_VERSION_MAJOR}") +set(SHARE_INSTALL_DIR + "share/${PROJECT_NAME_LOWER}-${PROJECT_VERSION_MAJOR}") + +# Dependencies +find_package(Flexiport) +# May need to link to the rt library +include(CheckFunctionExists) +set(CMAKE_REQUIRED_INCLUDES time.h) +set(CMAKE_REQUIRED_LIBRARIES rt) +CHECK_FUNCTION_EXISTS(clock_gettime HAVE_CLOCK_GETTIME) +set(CMAKE_REQUIRED_INCLUDES) +set(CMAKE_REQUIRED_LIBRARIES) + +# Subdirectories +add_subdirectory(cmake) +if(BUILD_DOCUMENTATION) + add_subdirectory(doc) +endif(BUILD_DOCUMENTATION) +if(BUILD_EXAMPLES) + add_subdirectory(examples) +endif(BUILD_EXAMPLES) +add_subdirectory(include) +add_subdirectory(src) +if(BUILD_PYTHON_BINDINGS) + add_subdirectory(python) +endif(BUILD_PYTHON_BINDINGS) + +# Package creation +include(InstallRequiredSystemLibraries) +set(PROJECT_EXECUTABLES ${EXAMPLE_EXECUTABLES}) +set(cpack_options "${PROJECT_BINARY_DIR}/cpack_options.cmake") +configure_file("${PROJECT_SOURCE_DIR}/cmake/cpack_options.cmake.in" + ${cpack_options} @ONLY) +set(CPACK_PROJECT_CONFIG_FILE ${cpack_options}) +include(CPack) + diff --git a/COPYING b/COPYING new file mode 100644 index 0000000..94a9ed0 --- /dev/null +++ b/COPYING @@ -0,0 +1,674 @@ + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. diff --git a/COPYING.LESSER b/COPYING.LESSER new file mode 100644 index 0000000..65c5ca8 --- /dev/null +++ b/COPYING.LESSER @@ -0,0 +1,165 @@ + GNU LESSER GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + + This version of the GNU Lesser General Public License incorporates +the terms and conditions of version 3 of the GNU General Public +License, supplemented by the additional permissions listed below. + + 0. Additional Definitions. + + As used herein, "this License" refers to version 3 of the GNU Lesser +General Public License, and the "GNU GPL" refers to version 3 of the GNU +General Public License. + + "The Library" refers to a covered work governed by this License, +other than an Application or a Combined Work as defined below. + + An "Application" is any work that makes use of an interface provided +by the Library, but which is not otherwise based on the Library. +Defining a subclass of a class defined by the Library is deemed a mode +of using an interface provided by the Library. + + A "Combined Work" is a work produced by combining or linking an +Application with the Library. The particular version of the Library +with which the Combined Work was made is also called the "Linked +Version". + + The "Minimal Corresponding Source" for a Combined Work means the +Corresponding Source for the Combined Work, excluding any source code +for portions of the Combined Work that, considered in isolation, are +based on the Application, and not on the Linked Version. + + The "Corresponding Application Code" for a Combined Work means the +object code and/or source code for the Application, including any data +and utility programs needed for reproducing the Combined Work from the +Application, but excluding the System Libraries of the Combined Work. + + 1. Exception to Section 3 of the GNU GPL. + + You may convey a covered work under sections 3 and 4 of this License +without being bound by section 3 of the GNU GPL. + + 2. Conveying Modified Versions. + + If you modify a copy of the Library, and, in your modifications, a +facility refers to a function or data to be supplied by an Application +that uses the facility (other than as an argument passed when the +facility is invoked), then you may convey a copy of the modified +version: + + a) under this License, provided that you make a good faith effort to + ensure that, in the event an Application does not supply the + function or data, the facility still operates, and performs + whatever part of its purpose remains meaningful, or + + b) under the GNU GPL, with none of the additional permissions of + this License applicable to that copy. + + 3. Object Code Incorporating Material from Library Header Files. + + The object code form of an Application may incorporate material from +a header file that is part of the Library. You may convey such object +code under terms of your choice, provided that, if the incorporated +material is not limited to numerical parameters, data structure +layouts and accessors, or small macros, inline functions and templates +(ten or fewer lines in length), you do both of the following: + + a) Give prominent notice with each copy of the object code that the + Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the object code with a copy of the GNU GPL and this license + document. + + 4. Combined Works. + + You may convey a Combined Work under terms of your choice that, +taken together, effectively do not restrict modification of the +portions of the Library contained in the Combined Work and reverse +engineering for debugging such modifications, if you also do each of +the following: + + a) Give prominent notice with each copy of the Combined Work that + the Library is used in it and that the Library and its use are + covered by this License. + + b) Accompany the Combined Work with a copy of the GNU GPL and this license + document. + + c) For a Combined Work that displays copyright notices during + execution, include the copyright notice for the Library among + these notices, as well as a reference directing the user to the + copies of the GNU GPL and this license document. + + d) Do one of the following: + + 0) Convey the Minimal Corresponding Source under the terms of this + License, and the Corresponding Application Code in a form + suitable for, and under terms that permit, the user to + recombine or relink the Application with a modified version of + the Linked Version to produce a modified Combined Work, in the + manner specified by section 6 of the GNU GPL for conveying + Corresponding Source. + + 1) Use a suitable shared library mechanism for linking with the + Library. A suitable mechanism is one that (a) uses at run time + a copy of the Library already present on the user's computer + system, and (b) will operate properly with a modified version + of the Library that is interface-compatible with the Linked + Version. + + e) Provide Installation Information, but only if you would otherwise + be required to provide such information under section 6 of the + GNU GPL, and only to the extent that such information is + necessary to install and execute a modified version of the + Combined Work produced by recombining or relinking the + Application with a modified version of the Linked Version. (If + you use option 4d0, the Installation Information must accompany + the Minimal Corresponding Source and Corresponding Application + Code. If you use option 4d1, you must provide the Installation + Information in the manner specified by section 6 of the GNU GPL + for conveying Corresponding Source.) + + 5. Combined Libraries. + + You may place library facilities that are a work based on the +Library side by side in a single library together with other library +facilities that are not Applications and are not covered by this +License, and convey such a combined library under terms of your +choice, if you do both of the following: + + a) Accompany the combined library with a copy of the same work based + on the Library, uncombined with any other library facilities, + conveyed under the terms of this License. + + b) Give prominent notice with the combined library that part of it + is a work based on the Library, and explaining where to find the + accompanying uncombined form of the same work. + + 6. Revised Versions of the GNU Lesser General Public License. + + The Free Software Foundation may publish revised and/or new versions +of the GNU Lesser General Public License from time to time. Such new +versions will be similar in spirit to the present version, but may +differ in detail to address new problems or concerns. + + Each version is given a distinguishing version number. If the +Library as you received it specifies that a certain numbered version +of the GNU Lesser General Public License "or any later version" +applies to it, you have the option of following the terms and +conditions either of that published version or of any later version +published by the Free Software Foundation. If the Library as you +received it does not specify a version number of the GNU Lesser +General Public License, you may choose any version of the GNU Lesser +General Public License ever published by the Free Software Foundation. + + If the Library as you received it specifies that a proxy can decide +whether future versions of the GNU Lesser General Public License shall +apply, that proxy's public statement of acceptance of any version is +permanent authorization for you to choose that version for the +Library. diff --git a/README.txt b/README.txt new file mode 100644 index 0000000..851e789 --- /dev/null +++ b/README.txt @@ -0,0 +1,16 @@ +HokuyoAIST +========== + +Hokuyo range sensor driver. + +This library provides a driver for Hokuyo laser scanner devices using +the SCIP protocol version 1 or 2. It has been tested with the Hokuyo +URG-04LX, UBG-04LX, UHG-08LX, UTM-30LX and UXM-30LX-E but it should work +with any scanner that conforms to these protocol versions, including the +URG-04LX-F01 and the URG-04LX-UG01 (Simple-URG). + +This software is developed at the National Institute of Advanced +Industrial Science and Technology. Approval number H22PRO-1195. This +software is licensed under the Lesser General Public License. See +COPYING.LESSER. + diff --git a/cmake/CMakeLists.txt b/cmake/CMakeLists.txt new file mode 100644 index 0000000..64b2557 --- /dev/null +++ b/cmake/CMakeLists.txt @@ -0,0 +1,17 @@ +set(PKG_DEPS flexiport) +set(PKG_LIBS -l${PROJECT_NAME_LOWER} -lrt) +set(pkg_conf_file ${CMAKE_CURRENT_BINARY_DIR}/hokuyoaist.pc) +configure_file(hokuyoaist.pc.in ${pkg_conf_file} @ONLY) +install(FILES ${pkg_conf_file} + DESTINATION ${LIB_INSTALL_DIR}/pkgconfig/ COMPONENT library) + +# Install CMake modules +set(cmake_config ${CMAKE_CURRENT_BINARY_DIR}/hokuyoaist-config.cmake) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/hokuyoaist-config.cmake.in + ${cmake_config} @ONLY) +set(cmake_version_config ${CMAKE_CURRENT_BINARY_DIR}/hokuyoaist-config-version.cmake) +configure_file(${CMAKE_CURRENT_SOURCE_DIR}/hokuyoaist-config-version.cmake.in + ${cmake_version_config} @ONLY) +set(cmake_mods ${cmake_config} ${cmake_version_config}) +install(FILES ${cmake_mods} DESTINATION ${SHARE_INSTALL_DIR} COMPONENT library) + diff --git a/cmake/cpack_options.cmake.in b/cmake/cpack_options.cmake.in new file mode 100644 index 0000000..3eb46bd --- /dev/null +++ b/cmake/cpack_options.cmake.in @@ -0,0 +1,53 @@ +set(CPACK_PACKAGE_NAME "@PROJECT_NAME@") +set(CPACK_PACKAGE_VERSION_MAJOR "@PROJECT_VERSION_MAJOR@") +set(CPACK_PACKAGE_VERSION_MINOR "@PROJECT_VERSION_MINOR@") +set(CPACK_PACKAGE_VERSION_PATCH "@PROJECT_VERSION_REVISION@") +set(CPACK_PACKAGE_DESCRIPTION_SUMMARY "@PROJECT_DESCRIPTION@") +set(CPACK_PACKAGE_VENDOR "@PROJECT_VENDOR@") +set(CPACK_PACKAGE_INSTALL_DIRECTORY "@PROJECT_NAME@") +set(CPACK_PACKAGE_FILE_NAME "@PROJECT_NAME@-@PROJECT_VERSION@") +set(CPACK_RESOURCE_FILE_LICENSE "@PROJECT_SOURCE_DIR@/COPYING.LESSER") + +set(CPACK_COMPONENTS_ALL library) +set(CPACK_COMPONENT_LIBRARY_DISPLAY_NAME "HokuyoAIST library") +set(CPACK_COMPONENT_LIBRARY_DESCRIPTION + "The HokuyoAIST library, for use with other applications.") +set(INSTALL_EXAMPLES @BUILD_EXAMPLES@) +if(INSTALL_EXAMPLES) + set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} examples) + set(CPACK_COMPONENT_EXAMPLES_DISPLAY_NAME "Examples") + set(CPACK_COMPONENT_EXAMPLES_DESCRIPTION + "Example source files and utilities.") + set(CPACK_COMPONENT_EXAMPLES_DEPENDS library) +endif(INSTALL_EXAMPLES) +set(INSTALL_DOCUMENTATION @BUILD_DOCUMENTATION@) +if(INSTALL_DOCUMENTATION) + set(CPACK_COMPONENTS_ALL ${CPACK_COMPONENTS_ALL} documentation) + set(CPACK_COMPONENT_DOCUMENTATION_DISPLAY_NAME "Documentation") + set(CPACK_COMPONENT_DOCUMENTATION_DESCRIPTION + "API documentation") + set(CPACK_COMPONENT_DOCUMENTATION_DEPENDS library) +endif(INSTALL_DOCUMENTATION) + +IF (WIN32) + set(CPACK_NSIS_HELP_LINK "https://github.com/gbiggs/hokuyoaist") + set(CPACK_NSIS_URL_INFO_ABOUT "https://github.com/gbiggs/hokuyoaist") + set(CPACK_NSIS_MODIFY_PATH ON) + set(CPACK_PACKAGE_EXECUTABLES @PROJECT_EXECUTABLES@) + if(INSTALL_DOCUMENTATION) + set(CPACK_NSIS_MENU_LINKS + "@CMAKE_INSTALL_PREFIX@/share/doc/@PROJECT_NAME_LOWER@-@PROJECT_VERSION_MAJOR@/html/index.html" + "API documentation") + endif(INSTALL_DOCUMENTATION) + string(REPLACE "/@PROJECT_NAME@" "" install_prefix_root + "@CMAKE_INSTALL_PREFIX@") + file(TO_NATIVE_PATH "${install_prefix_root}" install_prefix_root) + set(CPACK_NSIS_INSTALL_ROOT "${install_prefix_root}") + set(CPACK_NSIS_EXTRA_INSTALL_COMMANDS + " Rename \"$INSTDIR\\share\\@PROJECT_NAME_LOWER@-@PROJECT_VERSION_MAJOR@\\@PROJECT_NAME_LOWER@-config.cmake\" \"$INSTDIR\\@PROJECT_NAME_LOWER@-config.cmake\" + Rename \"$INSTDIR\\share\\@PROJECT_NAME_LOWER@-@PROJECT_VERSION_MAJOR@\\@PROJECT_NAME_LOWER@-config-version.cmake\" \"$INSTDIR\\@PROJECT_NAME_LOWER@-config-version.cmake\"") + set(CPACK_NSIS_EXTRA_UNINSTALL_COMMANDS + " Delete \"$INSTDIR\\@PROJECT_NAME_LOWER@-config.cmake\" + Delete \"$INSTDIR\\@PROJECT_NAME_LOWER@-config-version.cmake\"") +ENDIF (WIN32) + diff --git a/cmake/hokuyoaist-config-version.cmake.in b/cmake/hokuyoaist-config-version.cmake.in new file mode 100644 index 0000000..bdc28bf --- /dev/null +++ b/cmake/hokuyoaist-config-version.cmake.in @@ -0,0 +1,12 @@ +# Check the HokuyoAIST version provided. + +set(PACKAGE_VERSION @PROJECT_VERSION@) +if(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) + set(PACKAGE_VERSION_COMPATIBLE FALSE) +else(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) + set(PACKAGE_VERSION_COMPATIBLE TRUE) + if(PACKAGE_VERSION VERSION_EQUAL PACKAGE_FIND_VERSION) + set(PACKAGE_VERSION_EXACT TRUE) + endif(PACKAGE_VERSION VERSION_EQUAL PACKAGE_FIND_VERSION) +endif(PACKAGE_VERSION VERSION_LESS PACKAGE_FIND_VERSION) + diff --git a/cmake/hokuyoaist-config.cmake.in b/cmake/hokuyoaist-config.cmake.in new file mode 100644 index 0000000..d8f3440 --- /dev/null +++ b/cmake/hokuyoaist-config.cmake.in @@ -0,0 +1,42 @@ +# HokuyoAIST CMake config file +# +# This file sets the following variables: +# HokuyoAIST_FOUND - Always TRUE. +# HokuyoAIST_INCLUDE_DIRS - Directories containing the HokuyoAIST include files. +# HokuyoAIST_LIBRARIES - Libraries needed to use HokuyoAIST. +# HokuyoAIST_DEFINITIONS - Compiler flags for HokuyoAIST. +# HokuyoAIST_VERSION - The version of HokuyoAIST found. +# HokuyoAIST_VERSION_MAJOR - The major version of HokuyoAIST found. +# HokuyoAIST_VERSION_MINOR - The minor version of HokuyoAIST found. +# HokuyoAIST_VERSION_REVISION - The revision version of HokuyoAIST found. +# HokuyoAIST_VERSION_CANDIDATE - The candidate version of HokuyoAIST found. + +message(STATUS "Found HokuyoAIST-@PROJECT_VERSION@") +set(HokuyoAIST_FOUND TRUE) + +find_package(Flexiport REQUIRED) + +set(HokuyoAIST_INCLUDE_DIRS + "@CMAKE_INSTALL_PREFIX@/include/@PROJECT_NAME_LOWER@-@PROJECT_VERSION_MAJOR@" + ${Flexiport_INCLUDE_DIRS}) + +if(WIN32) + set(HokuyoAIST_LIBRARIES + "@CMAKE_INSTALL_PREFIX@/@LIB_INSTALL_DIR@/@CMAKE_SHARED_LIBRARY_PREFIX@@PROJECT_NAME_LOWER@@CMAKE_STATIC_LIBRARY_SUFFIX@" + ${Flexiport_LIBRARIES} + ) +else(WIN32) + set(HokuyoAIST_LIBRARIES + "@CMAKE_INSTALL_PREFIX@/@LIB_INSTALL_DIR@/@CMAKE_SHARED_LIBRARY_PREFIX@@PROJECT_NAME_LOWER@@CMAKE_SHARED_LIBRARY_SUFFIX@" + ${Flexiport_LIBRARIES} + ) +endif(WIN32) + +set(HokuyoAIST_DEFINITIONS ${Flexiport_DEFINITIONS}) + +set(HokuyoAIST_VERSION @PROJECT_VERSION@) +set(HokuyoAIST_VERSION_MAJOR @PROJECT_VERSION_MAJOR@) +set(HokuyoAIST_VERSION_MINOR @PROJECT_VERSION_MINOR@) +set(HokuyoAIST_VERSION_REVISION @PROJECT_VERSION_REVISION@) +set(HokuyoAIST_VERSION_CANDIDATE @PROJECT_VERSION_CANDIDATE@) + diff --git a/cmake/hokuyoaist.pc.in b/cmake/hokuyoaist.pc.in new file mode 100644 index 0000000..beac6be --- /dev/null +++ b/cmake/hokuyoaist.pc.in @@ -0,0 +1,13 @@ +# This file was generated by CMake for @PROJECT_NAME@ +prefix=@CMAKE_INSTALL_PREFIX@ +exec_prefix=${prefix} +libdir=${prefix}/@LIB_INSTALL_DIR@ +includedir=${prefix}/include + +Name: @PROJECT_NAME@ +Description: @PROJECT_DESCRIPTION@ +Version: @PROJECT_VERSION@ +Requires: @PKG_DEPS@ +Libs: -L${libdir} @PKG_LIBS@ +Cflags: -I${includedir}/@PROJECT_NAME_LOWER@-@PROJECT_VERSION_MAJOR@ + diff --git a/cmake/hokuyoaist_utils.cmake b/cmake/hokuyoaist_utils.cmake new file mode 100644 index 0000000..f18b665 --- /dev/null +++ b/cmake/hokuyoaist_utils.cmake @@ -0,0 +1,15 @@ +# Dissect the version specified in PROJECT_VERSION, placing the major, +# minor, revision and candidate components in PROJECT_VERSION_MAJOR, etc. +# _prefix: The prefix string for the version variable names. +macro(DISSECT_VERSION) + # Find version components + string(REGEX REPLACE "^([0-9]+).*" "\\1" + PROJECT_VERSION_MAJOR "${PROJECT_VERSION}") + string(REGEX REPLACE "^[0-9]+\\.([0-9]+).*" "\\1" + PROJECT_VERSION_MINOR "${PROJECT_VERSION}") + string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.([0-9]+)" "\\1" + PROJECT_VERSION_REVISION "${PROJECT_VERSION}") + string(REGEX REPLACE "^[0-9]+\\.[0-9]+\\.[0-9]+(.*)" "\\1" + PROJECT_VERSION_CANDIDATE "${PROJECT_VERSION}") +endmacro(DISSECT_VERSION) + diff --git a/cmake/uninstall_target.cmake.in b/cmake/uninstall_target.cmake.in new file mode 100644 index 0000000..e3db04e --- /dev/null +++ b/cmake/uninstall_target.cmake.in @@ -0,0 +1,19 @@ +if(NOT EXISTS "@PROJECT_BINARY_DIR@/install_manifest.txt") + message(FATAL_ERROR "Cannot find install manifest: \"@PROJECT_BINARY_DIR@/install_manifest.txt\"") +endif(NOT EXISTS "@PROJECT_BINARY_DIR@/install_manifest.txt") + +file(READ "@PROJECT_BINARY_DIR@/install_manifest.txt" files) +string(REGEX REPLACE "\n" ";" files "${files}") +foreach(file ${files}) + message(STATUS "Uninstalling \"$ENV{DESTDIR}${file}\"") + if(EXISTS "$ENV{DESTDIR}${file}") + exec_program("@CMAKE_COMMAND@" ARGS "-E remove \"$ENV{DESTDIR}${file}\"" + OUTPUT_VARIABLE rm_out RETURN_VALUE rm_retval) + if(NOT "${rm_retval}" STREQUAL 0) + message(FATAL_ERROR "Problem when removing \"$ENV{DESTDIR}${file}\"") + endif(NOT "${rm_retval}" STREQUAL 0) + else(EXISTS "$ENV{DESTDIR}${file}") + message(STATUS "File \"$ENV{DESTDIR}${file}\" does not exist.") + endif(EXISTS "$ENV{DESTDIR}${file}") +endforeach(file) + diff --git a/doc/CMakeLists.txt b/doc/CMakeLists.txt new file mode 100644 index 0000000..27bcc3b --- /dev/null +++ b/doc/CMakeLists.txt @@ -0,0 +1,38 @@ +find_package(Doxygen) +if(DOXYGEN_FOUND) + # Search for Sphinx + set(SPHINX_PATH "" CACHE PATH + "Path to the directory containing the sphinx-build program") + find_program(SPHINX_BUILD sphinx-build PATHS ${SPHINX_PATH}) + if(NOT SPHINX_BUILD) + message(FATAL_ERROR + "Sphinx was not found. Set SPHINX_PATH to the directory containing the sphinx-build executable, or disable BUILD_DOCUMENTATION.") + endif(NOT SPHINX_BUILD) + + set(html_dir "${CMAKE_CURRENT_BINARY_DIR}/html") + set(doxygen_dir "${html_dir}/doxygen") + file(MAKE_DIRECTORY ${html_dir}) + file(MAKE_DIRECTORY ${doxygen_dir}) + + # Doxygen part + set(doxyfile "${CMAKE_CURRENT_BINARY_DIR}/doxyfile") + configure_file(doxyfile.in ${doxyfile}) + add_custom_target(doxygen_doc ${DOXYGEN_EXECUTABLE} ${doxyfile}) + + # Sphinx part + set(conf_dir "${CMAKE_CURRENT_BINARY_DIR}/conf") + file(MAKE_DIRECTORY "${conf_dir}") + file(MAKE_DIRECTORY "${conf_dir}/_static") + set(conf_py "${conf_dir}/conf.py") + configure_file(conf.py.in ${conf_py}) + add_custom_target(sphinx_doc ALL sphinx-build -b html -c ${conf_dir} + ${CMAKE_CURRENT_SOURCE_DIR}/content ${CMAKE_CURRENT_BINARY_DIR}/html + DEPENDS doxygen_doc) + install(DIRECTORY "${CMAKE_CURRENT_BINARY_DIR}/html" DESTINATION + "share/doc/${PROJECT_NAME_LOWER}-${PROJECT_VERSION_MAJOR}" + COMPONENT doc) +else(DOXYGEN_FOUND) + message(FATAL_ERROR + "Doxygen was not found. Cannot build documentation. Disable BUILD_DOCUMENTATION to continue") +endif(DOXYGEN_FOUND) + diff --git a/doc/conf.py.in b/doc/conf.py.in new file mode 100644 index 0000000..2440aef --- /dev/null +++ b/doc/conf.py.in @@ -0,0 +1,216 @@ +# -*- coding: utf-8 -*- +# +# HokuyoAIST documentation build configuration file, created by +# sphinx-quickstart on Mon Aug 8 11:28:05 2011. +# +# This file is execfile()d with the current directory set to its containing dir. +# +# Note that not all possible configuration values are present in this +# autogenerated file. +# +# All configuration values have a default; values that are commented out +# serve to show the default. + +import sys, os + +# If extensions (or modules to document with autodoc) are in another directory, +# add these directories to sys.path here. If the directory is relative to the +# documentation root, use os.path.abspath to make it absolute, like shown here. +#sys.path.insert(0, os.path.abspath('.')) + +# -- General configuration ----------------------------------------------------- + +# If your documentation needs a minimal Sphinx version, state it here. +#needs_sphinx = '1.0' + +# Add any Sphinx extension module names here, as strings. They can be extensions +# coming with Sphinx (named 'sphinx.ext.*') or your custom ones. +extensions = ['breathe'] + +# Add any paths that contain templates here, relative to this directory. +templates_path = ['_templates'] + +# The suffix of source filenames. +source_suffix = '.txt' + +# The encoding of source files. +#source_encoding = 'utf-8-sig' + +# The master toctree document. +master_doc = 'index' + +# General information about the project. +project = u'HokuyoAIST' +copyright = u'2011, Geoffrey Biggs' + +# The version info for the project you're documenting, acts as replacement for +# |version| and |release|, also used in various other places throughout the +# built documents. +# +# The short X.Y version. +version = '2.0.0' +# The full version, including alpha/beta/rc tags. +release = '2.0.0' + +# The language for content autogenerated by Sphinx. Refer to documentation +# for a list of supported languages. +#language = None + +# There are two options for replacing |today|: either, you set today to some +# non-false value, then it is used: +#today = '' +# Else, today_fmt is used as the format for a strftime call. +#today_fmt = '%B %d, %Y' + +# List of patterns, relative to source directory, that match files and +# directories to ignore when looking for source files. +exclude_patterns = ['_build'] + +# The reST default role (used for this markup: `text`) to use for all documents. +#default_role = None + +# If true, '()' will be appended to :func: etc. cross-reference text. +#add_function_parentheses = True + +# If true, the current module name will be prepended to all description +# unit titles (such as .. function::). +#add_module_names = True + +# If true, sectionauthor and moduleauthor directives will be shown in the +# output. They are ignored by default. +#show_authors = False + +# The name of the Pygments (syntax highlighting) style to use. +pygments_style = 'sphinx' + +# A list of ignored prefixes for module index sorting. +#modindex_common_prefix = [] + + +# -- Options for HTML output --------------------------------------------------- + +# The theme to use for HTML and HTML Help pages. See the documentation for +# a list of builtin themes. +html_theme = 'default' + +# Theme options are theme-specific and customize the look and feel of a theme +# further. For a list of options available for each theme, see the +# documentation. +#html_theme_options = {} + +# Add any paths that contain custom themes here, relative to this directory. +#html_theme_path = [] + +# The name for this set of Sphinx documents. If None, it defaults to +# " v documentation". +#html_title = None + +# A shorter title for the navigation bar. Default is the same as html_title. +#html_short_title = None + +# The name of an image file (relative to this directory) to place at the top +# of the sidebar. +#html_logo = None + +# The name of an image file (within the static path) to use as favicon of the +# docs. This file should be a Windows icon file (.ico) being 16x16 or 32x32 +# pixels large. +#html_favicon = None + +# Add any paths that contain custom static files (such as style sheets) here, +# relative to this directory. They are copied after the builtin static files, +# so a file named "default.css" will overwrite the builtin "default.css". +html_static_path = ['_static'] + +# If not '', a 'Last updated on:' timestamp is inserted at every page bottom, +# using the given strftime format. +#html_last_updated_fmt = '%b %d, %Y' + +# If true, SmartyPants will be used to convert quotes and dashes to +# typographically correct entities. +#html_use_smartypants = True + +# Custom sidebar templates, maps document names to template names. +#html_sidebars = {} + +# Additional templates that should be rendered to pages, maps page names to +# template names. +#html_additional_pages = {} + +# If false, no module index is generated. +#html_domain_indices = True + +# If false, no index is generated. +#html_use_index = True + +# If true, the index is split into individual pages for each letter. +#html_split_index = False + +# If true, links to the reST sources are added to the pages. +#html_show_sourcelink = True + +# If true, "Created using Sphinx" is shown in the HTML footer. Default is True. +#html_show_sphinx = True + +# If true, "(C) Copyright ..." is shown in the HTML footer. Default is True. +#html_show_copyright = True + +# If true, an OpenSearch description file will be output, and all pages will +# contain a tag referring to it. The value of this option must be the +# base URL from which the finished HTML is served. +#html_use_opensearch = '' + +# This is the file name suffix for HTML files (e.g. ".xhtml"). +#html_file_suffix = None + +# Output file base name for HTML help builder. +htmlhelp_basename = 'HokuyoAISTdoc' + + +# -- Options for LaTeX output -------------------------------------------------- + +# The paper size ('letter' or 'a4'). +#latex_paper_size = 'letter' + +# The font size ('10pt', '11pt' or '12pt'). +#latex_font_size = '10pt' + +# Grouping the document tree into LaTeX files. List of tuples +# (source start file, target name, title, author, documentclass [howto/manual]). +latex_documents = [ + ('index', 'HokuyoAIST.tex', u'HokuyoAIST Documentation', + u'Geoffrey Biggs', 'manual'), +] + +# The name of an image file (relative to this directory) to place at the top of +# the title page. +#latex_logo = None + +# For "manual" documents, if this is true, then toplevel headings are parts, +# not chapters. +#latex_use_parts = False + +# If true, show page references after internal links. +#latex_show_pagerefs = False + +# If true, show URL addresses after external links. +#latex_show_urls = False + +# Additional stuff for the LaTeX preamble. +#latex_preamble = '' + +# Documents to append as an appendix to all manuals. +#latex_appendices = [] + +# If false, no module index is generated. +#latex_domain_indices = True + + +# -- Options for manual page output -------------------------------------------- + +# One entry per manual page. List of tuples +# (source start file, name, description, authors, manual section). +man_pages = [ + ('index', 'hokuyoaist', u'HokuyoAIST Documentation', + [u'Geoffrey Biggs'], 1) +] diff --git a/doc/content/index.txt b/doc/content/index.txt new file mode 100644 index 0000000..59b0b4a --- /dev/null +++ b/doc/content/index.txt @@ -0,0 +1,131 @@ +HokuyoAIST range sensor driver +============================== + +Contents: + +.. toctree:: + :maxdepth: 2 + +This library provides a driver for Hokuyo laser scanner devices using the SCIP +protocol version 1 or 2. It has been tested with the Hokuyo URG-04LX, UBG-04LX, +UHG-08LX, UTM-30LX and UXM-30LX-E but it should work with any scanner that +conforms to these protocol versions, including the URG-04LX-F01 and the +URG-04LX-UG01 (Simple-URG). + +For a full list of classes and functions, see the `API documentation`_. + +.. _`API Documentation`: + doxygen/html/index.html + + +Dependencies +============ + +This library uses the Flexiport library for communication with devices. That +library must be installed. If you are using the Windows binary installer, +Flexiport may be installed with HokuyoAIST. Otherwise, it must be installed +separately. + +Flexiport is available from GitHub_. + +.. _github: + http://www.gibhub.com/gbiggs/flexiport + + +Examples +======== + +If BUILD_EXAMPLES is enabled when the library is compiled and installed, +example sources are installed under "${prefix}/share/hokuyoaist-2/examples". +These can be used as a basis for creating your own software. + +Building +-------- + +The examples can be built by making a directory (anywhere on your system where +you have write permissions will do), changing to that directory and executing +CMake with the example's source directory as an argument. For example, if you +have installed HokuyoAIST into /usr/local, you could do the following: + +.. code-block:: bash + + $ cd ~ + $ mkdir hokuyoaist_examples + $ cd hokuyoaist_examples + $ ccmake /usr/local/share/hokuyoaist-2/examples/ + +Running +------- + +The examples requires that you specify suitable options for the underlying +Flexiport object used to communicate with the laser scanner. At a minimum, a +type will be required. Other options, such as a baud rate, may also be +specified. For example: + +.. code-block:: bash + + ./HokuyoAISTExample_example -o type=serial,device=/dev/ttyACM0,timeout=1 -b 19200 + +This will start the example, looking for the laser on port /dev/ttyACM0 and +using a timeout of one second, and connecting at a baud rate of 19200bps. + +See the flexiport documentation for more details on available port types and +their options. Specify -h or -? to see a list of available options for the +example. + +Some log file pairs for use with the LogReaderPort port type are also included. +Using a log file pair means the hardware (in this case, the laser scanner) does +not need to be present to execute the example. You can use the log file pairs +like this: + +.. code-block:: bash + + ./HokuyoAISTExample_example -o type=logreader,file=example_urg04lx.log,timeout=1 + +See the Flexiport LogReaderPort object documentation for more options that can +be used with log file pairs. See the Flexiport LogWriterPort object +documentation for details on how to make your own log file pair for testing +your programs. + +The available log file pairs are: + +- example_urg_04lx.log(r,w): URG-04LX (Classic-URG) +- example_utm_30lx.log(r,w): UTM-30LX (Top-URG) +- example_uxm_30lx_e.log(r,w): UXM-30LX-E + +GetID Example +------------- + +(Developed by Luiz Mirisola.) + +This example is a stripped down version of the hokuyoaist_example that +connects to the device, prints out its serial number, and closes the device. + +It is useful for writing udev scripts to create permanent symlinks to your +lasers, in the case where you have more than one laser, as the serial number is +not otherwise available through udev. + +The file, "96-hokuyo.rules," should be copied to /etc/udev/rules.d, followed by +restarting the udev deamon. + +When a Hokuyo sensor is plugged, the /dev/ttyACM* device will be generated as +before, and a symbolic link called hokuyo_XXXXXX will also be generated, where +XXXXX is the laser serial number. + +Note for Windows users +---------------------- + +Because Windows lacks a readily-available implementation of getopt, command line +options are not available for the example on this platform. Hard-coded options +will be used instead; change them in the source file and recompile if you need +to change the port options. + + +License +======= + +This software is developed at the National Institute of Advanced +Industrial Science and Technology. Approval number H22PRO-1195. This +software is licensed under the Lesser General Public License. See +COPYING.LESSER. + diff --git a/doc/doxyfile.in b/doc/doxyfile.in new file mode 100644 index 0000000..3d7e00b --- /dev/null +++ b/doc/doxyfile.in @@ -0,0 +1,297 @@ +#--------------------------------------------------------------------------- +# Project related configuration options +#--------------------------------------------------------------------------- +DOXYFILE_ENCODING = UTF-8 +PROJECT_NAME = "@PROJECT_NAME@" +PROJECT_NUMBER = @PROJECT_VERSION@ +OUTPUT_DIRECTORY = "@doxygen_dir@" +CREATE_SUBDIRS = NO +OUTPUT_LANGUAGE = English +BRIEF_MEMBER_DESC = YES +REPEAT_BRIEF = YES +ABBREVIATE_BRIEF = "The $name class" \ + "The $name widget" \ + "The $name file" \ + is \ + provides \ + specifies \ + contains \ + represents \ + a \ + an \ + the +ALWAYS_DETAILED_SEC = NO +INLINE_INHERITED_MEMB = NO +FULL_PATH_NAMES = YES +STRIP_FROM_PATH = +STRIP_FROM_INC_PATH = @PROJECT_SOURCE_DIR@/include +SHORT_NAMES = NO +JAVADOC_AUTOBRIEF = YES +QT_AUTOBRIEF = NO +MULTILINE_CPP_IS_BRIEF = NO +INHERIT_DOCS = YES +SEPARATE_MEMBER_PAGES = NO +TAB_SIZE = 2 +ALIASES = +OPTIMIZE_OUTPUT_FOR_C = NO +OPTIMIZE_OUTPUT_JAVA = NO +OPTIMIZE_FOR_FORTRAN = NO +OPTIMIZE_OUTPUT_VHDL = NO +EXTENSION_MAPPING = +BUILTIN_STL_SUPPORT = NO +CPP_CLI_SUPPORT = NO +SIP_SUPPORT = NO +IDL_PROPERTY_SUPPORT = YES +DISTRIBUTE_GROUP_DOC = NO +SUBGROUPING = YES +TYPEDEF_HIDES_STRUCT = NO +SYMBOL_CACHE_SIZE = 0 + +#--------------------------------------------------------------------------- +# Build related configuration options +#--------------------------------------------------------------------------- +EXTRACT_ALL = YES +EXTRACT_PRIVATE = NO +EXTRACT_STATIC = NO +EXTRACT_LOCAL_CLASSES = YES +EXTRACT_LOCAL_METHODS = NO +EXTRACT_ANON_NSPACES = NO +HIDE_UNDOC_MEMBERS = NO +HIDE_UNDOC_CLASSES = NO +HIDE_FRIEND_COMPOUNDS = NO +HIDE_IN_BODY_DOCS = NO +INTERNAL_DOCS = NO +CASE_SENSE_NAMES = NO +HIDE_SCOPE_NAMES = NO +SHOW_INCLUDE_FILES = YES +FORCE_LOCAL_INCLUDES = NO +INLINE_INFO = YES +SORT_MEMBER_DOCS = YES +SORT_BRIEF_DOCS = NO +SORT_MEMBERS_CTORS_1ST = NO +SORT_GROUP_NAMES = NO +SORT_BY_SCOPE_NAME = NO +GENERATE_TODOLIST = YES +GENERATE_TESTLIST = YES +GENERATE_BUGLIST = YES +GENERATE_DEPRECATEDLIST= YES +ENABLED_SECTIONS = +MAX_INITIALIZER_LINES = 30 +SHOW_USED_FILES = YES +SHOW_DIRECTORIES = YES +SHOW_FILES = YES +SHOW_NAMESPACES = YES +FILE_VERSION_FILTER = +LAYOUT_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to warning and progress messages +#--------------------------------------------------------------------------- +QUIET = YES +WARNINGS = YES +WARN_IF_UNDOCUMENTED = YES +WARN_IF_DOC_ERROR = YES +WARN_NO_PARAMDOC = NO +WARN_FORMAT = "$file:$line: $text" +WARN_LOGFILE = + +#--------------------------------------------------------------------------- +# configuration options related to the input files +#--------------------------------------------------------------------------- +INPUT = "@PROJECT_SOURCE_DIR@" \ + "@PROJECT_SOURCE_DIR@/doc" +INPUT_ENCODING = UTF-8 +FILE_PATTERNS = *.h \ + *.hpp \ + *.doxy +RECURSIVE = YES +EXCLUDE = "@PROJECT_SOURCE_DIR@/cmake" \ + "@PROJECT_SOURCE_DIR@/build" +EXCLUDE_SYMLINKS = YES +EXCLUDE_PATTERNS = +EXCLUDE_SYMBOLS = +EXAMPLE_PATH = +EXAMPLE_PATTERNS = * +EXAMPLE_RECURSIVE = NO +IMAGE_PATH = +INPUT_FILTER = +FILTER_PATTERNS = +FILTER_SOURCE_FILES = NO + +#--------------------------------------------------------------------------- +# configuration options related to source browsing +#--------------------------------------------------------------------------- +SOURCE_BROWSER = YES +INLINE_SOURCES = NO +STRIP_CODE_COMMENTS = YES +REFERENCED_BY_RELATION = NO +REFERENCES_RELATION = NO +REFERENCES_LINK_SOURCE = YES +USE_HTAGS = NO +VERBATIM_HEADERS = YES + +#--------------------------------------------------------------------------- +# configuration options related to the alphabetical class index +#--------------------------------------------------------------------------- +ALPHABETICAL_INDEX = YES +COLS_IN_ALPHA_INDEX = 5 +IGNORE_PREFIX = + +#--------------------------------------------------------------------------- +# configuration options related to the HTML output +#--------------------------------------------------------------------------- +GENERATE_HTML = YES +HTML_OUTPUT = html +HTML_FILE_EXTENSION = .html +HTML_HEADER = +HTML_FOOTER = +HTML_STYLESHEET = +HTML_COLORSTYLE_HUE = 220 +HTML_COLORSTYLE_SAT = 100 +HTML_COLORSTYLE_GAMMA = 80 +HTML_TIMESTAMP = YES +HTML_ALIGN_MEMBERS = YES +HTML_DYNAMIC_SECTIONS = NO +GENERATE_DOCSET = YES +DOCSET_FEEDNAME = "Doxygen generated docs" +DOCSET_BUNDLE_ID = @PROJECT_NAME_LOWER@.gbiggs +DOCSET_PUBLISHER_ID = @PROJECT_NAME_LOWER@.gbiggs.Publisher +DOCSET_PUBLISHER_NAME = Geoffrey Biggs/AIST +GENERATE_HTMLHELP = YES +CHM_FILE = "@PROJECT_NAME@-@PROJECT_VERSION_MAJOR@.@PROJECT_VERSION_MINOR@.chm" +HHC_LOCATION = "@HTML_HELP_COMPILER@" +GENERATE_CHI = NO +CHM_INDEX_ENCODING = +BINARY_TOC = NO +TOC_EXPAND = NO +GENERATE_QHP = NO +QCH_FILE = +QHP_NAMESPACE = @PROJECT_NAME_LOWER@.gbiggs.Project +QHP_VIRTUAL_FOLDER = doc +QHP_CUST_FILTER_NAME = +QHP_CUST_FILTER_ATTRS = +QHP_SECT_FILTER_ATTRS = +QHG_LOCATION = +GENERATE_ECLIPSEHELP = NO +ECLIPSE_DOC_ID = @PROJECT_NAME_LOWER@.gbiggs.Project +DISABLE_INDEX = NO +ENUM_VALUES_PER_LINE = 4 +GENERATE_TREEVIEW = NO +USE_INLINE_TREES = NO +TREEVIEW_WIDTH = 250 +EXT_LINKS_IN_WINDOW = NO +FORMULA_FONTSIZE = 10 +FORMULA_TRANSPARENT = YES +SEARCHENGINE = YES +SERVER_BASED_SEARCH = NO + +#--------------------------------------------------------------------------- +# configuration options related to the LaTeX output +#--------------------------------------------------------------------------- +GENERATE_LATEX = NO +LATEX_OUTPUT = latex +LATEX_CMD_NAME = latex +MAKEINDEX_CMD_NAME = makeindex +COMPACT_LATEX = NO +PAPER_TYPE = a4wide +EXTRA_PACKAGES = +LATEX_HEADER = +PDF_HYPERLINKS = YES +USE_PDFLATEX = YES +LATEX_BATCHMODE = NO +LATEX_HIDE_INDICES = NO +LATEX_SOURCE_CODE = NO + +#--------------------------------------------------------------------------- +# configuration options related to the RTF output +#--------------------------------------------------------------------------- +GENERATE_RTF = NO +RTF_OUTPUT = rtf +COMPACT_RTF = NO +RTF_HYPERLINKS = NO +RTF_STYLESHEET_FILE = +RTF_EXTENSIONS_FILE = + +#--------------------------------------------------------------------------- +# configuration options related to the man page output +#--------------------------------------------------------------------------- +GENERATE_MAN = NO +MAN_OUTPUT = man +MAN_EXTENSION = .3 +MAN_LINKS = NO + +#--------------------------------------------------------------------------- +# configuration options related to the XML output +#--------------------------------------------------------------------------- +GENERATE_XML = NO +XML_OUTPUT = xml +XML_SCHEMA = +XML_DTD = +XML_PROGRAMLISTING = YES + +#--------------------------------------------------------------------------- +# configuration options for the AutoGen Definitions output +#--------------------------------------------------------------------------- +GENERATE_AUTOGEN_DEF = NO + +#--------------------------------------------------------------------------- +# configuration options related to the Perl module output +#--------------------------------------------------------------------------- +GENERATE_PERLMOD = NO +PERLMOD_LATEX = NO +PERLMOD_PRETTY = YES +PERLMOD_MAKEVAR_PREFIX = + +#--------------------------------------------------------------------------- +# Configuration options related to the preprocessor +#--------------------------------------------------------------------------- +ENABLE_PREPROCESSING = YES +MACRO_EXPANSION = NO +EXPAND_ONLY_PREDEF = NO +SEARCH_INCLUDES = YES +INCLUDE_PATH = +INCLUDE_FILE_PATTERNS = *.h +PREDEFINED = +EXPAND_AS_DEFINED = +SKIP_FUNCTION_MACROS = YES + +#--------------------------------------------------------------------------- +# Configuration::additions related to external references +#--------------------------------------------------------------------------- +TAGFILES = +GENERATE_TAGFILE = +ALLEXTERNALS = NO +EXTERNAL_GROUPS = YES +PERL_PATH = /usr/bin/perl + +#--------------------------------------------------------------------------- +# Configuration options related to the dot tool +#--------------------------------------------------------------------------- +CLASS_DIAGRAMS = YES +MSCGEN_PATH = +HIDE_UNDOC_RELATIONS = YES +HAVE_DOT = YES +DOT_NUM_THREADS = 0 +DOT_FONTNAME = FreeSans.ttf +DOT_FONTSIZE = 10 +DOT_FONTPATH = +CLASS_GRAPH = YES +COLLABORATION_GRAPH = YES +GROUP_GRAPHS = YES +UML_LOOK = NO +TEMPLATE_RELATIONS = NO +INCLUDE_GRAPH = YES +INCLUDED_BY_GRAPH = YES +CALL_GRAPH = NO +CALLER_GRAPH = NO +GRAPHICAL_HIERARCHY = YES +DIRECTORY_GRAPH = YES +DOT_IMAGE_FORMAT = png +DOT_PATH = +DOTFILE_DIRS = +DOT_GRAPH_MAX_NODES = 50 +MAX_DOT_GRAPH_DEPTH = 0 +DOT_TRANSPARENT = NO +DOT_MULTI_TARGETS = NO +GENERATE_LEGEND = YES +DOT_CLEANUP = YES diff --git a/examples/96-hokuyo.rules b/examples/96-hokuyo.rules new file mode 100644 index 0000000..2106174 --- /dev/null +++ b/examples/96-hokuyo.rules @@ -0,0 +1 @@ +ACTION=="add|change", KERNEL=="ttyACM*", ATTRS{idVendor}=="15d1", PROGRAM="/usr/local/bin/hokuyoaist_getid -o type=serial,timeout=1,device=%N", SYMLINK+="hokuyo_%c" diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt new file mode 100644 index 0000000..0ee41d4 --- /dev/null +++ b/examples/CMakeLists.txt @@ -0,0 +1,42 @@ +include_directories(${PROJECT_SOURCE_DIR}/include) +include_directories(${PROJECT_BINARY_DIR}/include) +include_directories(${Flexiport_INCLUDE_DIRS}) + +# Laser data reader example +add_executable(${PROJECT_NAME_LOWER}_example example.cpp) +target_link_libraries(${PROJECT_NAME_LOWER}_example + ${PROJECT_NAME_LOWER}) +install(TARGETS ${PROJECT_NAME_LOWER}_example + DESTINATION ${BIN_INSTALL_DIR} + COMPONENT examples) + +# Get laser ID sample +add_executable(${PROJECT_NAME_LOWER}_getid getid.cpp) +target_link_libraries(${PROJECT_NAME_LOWER}_getid + ${PROJECT_NAME_LOWER}) +install(TARGETS ${PROJECT_NAME_LOWER}_getid + DESTINATION ${BIN_INSTALL_DIR} + COMPONENT examples) + +# Install example sources +install(FILES CMakeLists.txt.example + DESTINATION ${SHARE_INSTALL_DIR}/examples + RENAME CMakeLists.txt + COMPONENT examples) +install(FILES example.cpp getid.cpp + example_urg_04lx.logr example_urg_04lx.logw + example_utm_30lx.logr example_utm_30lx.logw + example_uxm_30lx_e.logr example_uxm_30lx_e.logw + 96-hokuyo.rules + DESTINATION ${SHARE_INSTALL_DIR}/examples + COMPONENT examples) +install(FILES example.readme + DESTINATION ${SHARE_INSTALL_DIR}/examples + RENAME README.txt + COMPONENT examples) + +# For packaging +set(EXAMPLE_EXECUTABLES ${PROJECT_NAME_LOWER}_example HokuyoAIST_Example + ${PROJECT_NAME_LOWER}_getid GetID + PARENT_SCOPE) + diff --git a/examples/CMakeLists.txt.example b/examples/CMakeLists.txt.example new file mode 100644 index 0000000..9359449 --- /dev/null +++ b/examples/CMakeLists.txt.example @@ -0,0 +1,21 @@ +cmake_minimum_required(VERSION 2.8) + +project(HokuyoAISTExample) + +# Search for dependencies +find_package(HokuyoAIST) + +# Set compiler flags +include_directories(${HokuyoAIST_INCLUDE_DIRS}) +add_definitions(${HokuyoAIST_DEFINITIONS}) + +# Get laser data example +add_executable(${PROJECT_NAME}_example example.cpp) +target_link_libraries(${PROJECT_NAME}_example + ${HokuyoAIST_LIBRARIES}) + +# Get laser ID example +add_executable(${PROJECT_NAME}_getid getid.cpp) +target_link_libraries(${PROJECT_NAME}_getid + ${HokuyoAIST_LIBRARIES}) + diff --git a/examples/example.cpp b/examples/example.cpp new file mode 100644 index 0000000..69b8a8e --- /dev/null +++ b/examples/example.cpp @@ -0,0 +1,248 @@ +/* HokuyoAIST + * + * Laser data read example. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include +#include +#include +#include +#include +#include + + +int main(int argc, char **argv) +{ + std::string port_options("type=serial,device=/dev/ttyACM0,timeout=1"); + double start_angle(0.0), end_angle(0.0); + int first_step(-1), last_step(-1); + int multiecho_mode(0); + unsigned int baud(19200), speed(0), cluster_count(1); + bool get_intensities(false), get_new(false), verbose(false); + +#if defined(WIN32) + port_options = "type=serial,device=COM4,timeout=1"; +#else + int opt; + // Get some options from the command line + while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:u:vh")) != -1) + { + switch(opt) + { + case 'b': + sscanf(optarg, "%d", &baud); + break; + case 'c': + sscanf(optarg, "%d", &cluster_count); + break; + case 'e': + sscanf(optarg, "%lf", &end_angle); + break; + case 'f': + sscanf(optarg, "%d", &first_step); + break; + case 'i': + get_intensities = true; + break; + case 'l': + sscanf(optarg, "%d", &last_step); + break; + case 'm': + sscanf(optarg, "%d", &speed); + break; + case 'n': + get_new = true; + break; + case 'o': + port_options = optarg; + break; + case 's': + sscanf(optarg, "%lf", &start_angle); + break; + case 'u': + sscanf(optarg, "%d", &multiecho_mode); + break; + case 'v': + verbose = true; + break; + case '?': + case 'h': + default: + std::cout << "Usage: " << argv[0] << " [options]\n\n"; + std::cout << "-b baud\t\tBaud rate to set the laser to " + "*after* connecting.\n"; + std::cout << "-c count\tCluster count.\n"; + std::cout << "-e angle\tEnd angle to get ranges to.\n"; + std::cout << "-f step\t\tFirst step to get ranges from.\n"; + std::cout << "-i\t\tGet intensity data along with ranges.\n"; + std::cout << "-l step\t\tLast step to get ranges to.\n"; + std::cout << "-m speed\tMotor speed.\n"; + std::cout << + "-n\t\tGet new ranges instead of latest ranges.\n"; + std::cout << + "-o options\tPort options (see flexiport library).\n"; + std::cout << "-s angle\tStart angle to get ranges from.\n"; + std::cout << "-u mode\tMulti-echo detection:\n"; + std::cout << "\t\t0: Off (default), 1: Front, 2: Middle, " + "3: Rear, 4: Average\n"; + std::cout << + "-v\t\tPut the hokuyoaist library into verbose mode.\n"; + return 1; + } + } +#endif // defined(WIN32) + + try + { + hokuyoaist::Sensor laser; // Laser scanner object + // Set the laser to verbose mode (so we see more information in the + // console) + if(verbose) + laser.set_verbose(true); + + // Open the laser + laser.open(port_options); + + // Calibrate the laser time stamp + std::cout << "Calibrating laser time\n"; + laser.calibrate_time(); + std::cout << "Calculated offset: " << laser.time_offset() << "ns\n"; + std::cout << "Calculated drift rate: " << laser.drift_rate() << '\n'; + std::cout << "Calculated skew alpha: " << laser.skew_alpha() << '\n'; + + // Turn the laser on + laser.set_power(true); + // Set the baud rate + try + { + laser.set_baud(baud); + } + catch(hokuyoaist::BaudrateError &e) + { + std::cerr << "Failed to change baud rate: " << e.what() << '\n'; + } + catch(hokuyoaist::ResponseError &e) + { + std::cerr << "Failed to change baud rate: " << e.what() << '\n'; + } + catch(...) + { + std::cerr << "Failed to change baud rate\n"; + } + // Set the motor speed + try + { + laser.set_motor_speed(speed); + } + catch(hokuyoaist::MotorSpeedError &e) + { + std::cerr << "Failed to set motor speed: " << e.what() << '\n'; + } + catch(hokuyoaist::ResponseError &e) + { + std::cerr << "Failed to set motor speed: " << e.what() << '\n'; + } + // Set multiecho mode + switch(multiecho_mode) + { + case 1: + laser.set_multiecho_mode(hokuyoaist::ME_FRONT); + break; + case 2: + laser.set_multiecho_mode(hokuyoaist::ME_MIDDLE); + break; + case 3: + laser.set_multiecho_mode(hokuyoaist::ME_REAR); + break; + case 4: + laser.set_multiecho_mode(hokuyoaist::ME_AVERAGE); + break; + case 0: + default: + laser.set_multiecho_mode(hokuyoaist::ME_OFF); + break; + } + + // Get some laser info + std::cout << "Laser sensor information:\n"; + hokuyoaist::SensorInfo info; + laser.get_sensor_info(info); + std::cout << info.as_string(); + + // Get range data + hokuyoaist::ScanData data; + if((first_step == -1 && last_step == -1) && + (start_angle == 0.0 && end_angle == 0.0)) + { + // Get all ranges + if(get_new) + laser.get_new_ranges(data, -1, -1, cluster_count); + else if(get_intensities) + laser.get_new_ranges_intensities(data, -1, -1, cluster_count); + else + laser.get_ranges(data, -1, -1, cluster_count); + } + else if(first_step != -1 || last_step != -1) + { + // Get by step + if(get_new) + laser.get_new_ranges(data, first_step, last_step, + cluster_count); + else if(get_intensities) + laser.get_new_ranges_intensities(data, first_step, last_step, + cluster_count); + else + laser.get_ranges(data, first_step, last_step, cluster_count); + } + else + { + // Get by angle + if(get_new) + laser.get_new_ranges_by_angle(data, start_angle, end_angle, + cluster_count); + else if(get_intensities) + laser.get_new_ranges_intensities_by_angle(data, start_angle, + end_angle, cluster_count); + else + laser.get_ranges_by_angle(data, start_angle, end_angle, + cluster_count); + } + + std::cout << "Measured data:\n"; + std::cout << data.as_string(); + + // Close the laser + laser.close(); + } + catch(hokuyoaist::BaseError &e) + { + std::cerr << "Caught exception: " << e.what() << '\n'; + return 1; + } + + return 0; +} + diff --git a/examples/example.readme b/examples/example.readme new file mode 100644 index 0000000..025d761 --- /dev/null +++ b/examples/example.readme @@ -0,0 +1,56 @@ +Building +-------- + +The examples can be built by making a directory (anywhere on your system where +you have write permissions will do), changing to that directory and executing +CMake with the example's source directory as an argument. For example, if you +have installed HokuyoAIST into /usr/local, you could do the following: + +$ cd ~ +$ mkdir hokuyoaist_examples +$ cd hokuyoaist_examples +$ ccmake /usr/local/share/hokuyoaist-2/examples/ + +Running +------- + +The examples requires that you specify suitable options for the underlying +Flexiport object used to communicate with the laser scanner. At a minimum, a +type will be required. Other options, such as a baud rate, may also be +specified. For example: + +./hokuyoaist_example -o type=serial,device=/dev/ttyACM0,timeout=1 -b 19200 + +This will start the example, looking for the laser on port /dev/ttyACM0 and +using a timeout of one second, and connecting at a baud rate of 19200bps. + +See the flexiport documentation for more details on available port types and +their options. Specify -h or -? to see a list of available options for the +example. + +Some log file pairs for use with the LogReaderPort port type are also included. +Using a log file pair means the hardware (in this case, the laser scanner) does +not need to be present to execute the example. You can use the log file pairs +like this: + +./hokuyoaist_example -o type=logreader,file=example_urg04lx.log,timeout=1 + +See the Flexiport LogReaderPort object documentation for more options that can +be used with log file pairs. See the Flexiport LogWriterPort object +documentation for details on how to make your own log file pair for testing +your programs. + +The available log file pairs are: + +- example_urg_04lx.log(r,w): URG-04LX (Classic-URG) +- example_utm_30lx.log(r,w): UTM-30LX (Top-URG) +- example_uxm_30lx_e.log(r,w): UXM-30LX-E + +Note for Windows users +---------------------- + +Because Windows lacks a readily-available implementation of getopt, command line +options are not available for the example on this platform. Hard-coded options +will be used instead; change them in the source file and recompile if you need +to change the port options. + diff --git a/examples/example_urg_04lx.logr b/examples/example_urg_04lx.logr new file mode 100644 index 0000000..58dda1a Binary files /dev/null and b/examples/example_urg_04lx.logr differ diff --git a/examples/example_urg_04lx.logw b/examples/example_urg_04lx.logw new file mode 100644 index 0000000..c8cb30b Binary files /dev/null and b/examples/example_urg_04lx.logw differ diff --git a/examples/example_utm_30lx.logr b/examples/example_utm_30lx.logr new file mode 100644 index 0000000..8d9341c Binary files /dev/null and b/examples/example_utm_30lx.logr differ diff --git a/examples/example_utm_30lx.logw b/examples/example_utm_30lx.logw new file mode 100644 index 0000000..02340ab Binary files /dev/null and b/examples/example_utm_30lx.logw differ diff --git a/examples/example_uxm_30lx_e.logr b/examples/example_uxm_30lx_e.logr new file mode 100644 index 0000000..f2a3999 Binary files /dev/null and b/examples/example_uxm_30lx_e.logr differ diff --git a/examples/example_uxm_30lx_e.logw b/examples/example_uxm_30lx_e.logw new file mode 100644 index 0000000..b7648ba Binary files /dev/null and b/examples/example_uxm_30lx_e.logw differ diff --git a/examples/getid.cpp b/examples/getid.cpp new file mode 100644 index 0000000..5eec322 --- /dev/null +++ b/examples/getid.cpp @@ -0,0 +1,98 @@ +/* HokuyoAIST + * + * Utility to print a sensor's ID, by Luiz Mirisola. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include +#include +#include +#include +#include + + +int main(int argc, char **argv) +{ + std::string port_options="type=serial,device=/dev/ttyACM0,timeout=1"; + bool verbose=false; + +#if defined (WIN32) + port_options = "type=serial,device=COM3,timeout=1"; +#else + int opt; + // Get some options from the command line + while((opt = getopt(argc, argv, "b:c:e:f:il:m:no:s:vh")) != -1) + { + switch(opt) + { + case 'o': + port_options = optarg; + break; + case 'v': + verbose = true; + break; + case '?': + case 'h': + default: + std::cout << "Usage: " << argv[0] << " [options]\n\n"; + std::cout << + "-o options\tPort options (see flexiport library).\n"; + std::cout << + "-v\t\tPut the hokuyoaist library into verbose mode.\n"; + return 1; + } + } +#endif // defined (WIN32) + + try + { + hokuyoaist::Sensor laser; // Laser scanner object + // Set the laser to verbose mode (so we see more information in the + // console) + if(verbose) + laser.set_verbose(true); + + // Open the laser + laser.open(port_options); + // Turn the laser on + laser.set_power(true); + + // Get some laser info + hokuyoaist::SensorInfo info; + laser.get_sensor_info(info); + std::cout << info.serial << '\n'; + + // Close the laser + laser.close(); + } + catch(hokuyoaist::BaseError &e) + { + std::cerr << "Caught exception: " << e.what() << '\n'; + return 1; + } + + return 0; +} + diff --git a/include/CMakeLists.txt b/include/CMakeLists.txt new file mode 100644 index 0000000..177e503 --- /dev/null +++ b/include/CMakeLists.txt @@ -0,0 +1,2 @@ +add_subdirectory(hokuyoaist) + diff --git a/include/hokuyoaist/CMakeLists.txt b/include/hokuyoaist/CMakeLists.txt new file mode 100644 index 0000000..00089f3 --- /dev/null +++ b/include/hokuyoaist/CMakeLists.txt @@ -0,0 +1,10 @@ +set(hdrs hokuyoaist.h + hokuyo_errors.h + scan_data.h + sensor.h + sensor_info.h + utils.h + ) +install(FILES ${hdrs} DESTINATION ${INC_INSTALL_DIR}/${PROJECT_NAME_LOWER} + COMPONENT library) + diff --git a/include/hokuyoaist/hokuyo_errors.h b/include/hokuyoaist/hokuyo_errors.h new file mode 100644 index 0000000..f2a8226 --- /dev/null +++ b/include/hokuyoaist/hokuyo_errors.h @@ -0,0 +1,677 @@ +/* HokuyoAIST + * + * Header file for exceptions. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef HOKUYO_ERRORS_H__ +#define HOKUYO_ERRORS_H__ + +#include + +#if defined(WIN32) + typedef unsigned char uint8_t; + typedef unsigned int uint32_t; + #if defined(HOKUYOAIST_STATIC) + #define HOKUYOAIST_EXPORT + #elif defined(hokuyoaist_EXPORTS) + #define HOKUYOAIST_EXPORT __declspec(dllexport) + #else + #define HOKUYOAIST_EXPORT __declspec(dllimport) + #endif +#else + #include + #define HOKUYOAIST_EXPORT +#endif + +/** @ingroup library_hokuyoaist +@{ +*/ + +namespace hokuyoaist +{ + +/// Translates a SCIP2 error code into a string. +std::string scip2_error_to_string(char const* const error, + char const* const cmd); + +/// Translates an error description code into a string. +std::string desc_code_to_string(unsigned int code); + + +/// General error class. +class HOKUYOAIST_EXPORT BaseError : public std::exception +{ + public: + /** @brief Hokuyo error constructor. + + @param desc_code Index into the error descriptions string table. + @param error_type The error as a string. + */ + BaseError(unsigned int desc_code, char const* error_type); + BaseError(BaseError const& rhs); + virtual ~BaseError() throw() {}; + + virtual unsigned int desc_code() const throw() + { return desc_code_; } + + virtual char const* error_type() const throw() + { return error_type_; } + + virtual const char* what() throw(); + + protected: + /** Description code for use with the error string table. */ + unsigned int desc_code_; + + /** Formatted description of the error. */ + std::stringstream ss; + /** String representation of the error. */ + char error_type_[32]; +}; //class BaseError + + +/// Logic error class +class HOKUYOAIST_EXPORT LogicError : public BaseError +{ + public: + /** @brief Logic error constructor. + + @param desc_code Index into the error descriptions string table. */ + LogicError(unsigned int desc_code) + : BaseError(desc_code, "LogicError") + {} + LogicError(unsigned int desc_code, char const* error_type) + : BaseError(desc_code, error_type) + {} + virtual ~LogicError() throw() {}; +}; // class LogicError + + +/// Runtime error class +class HOKUYOAIST_EXPORT RuntimeError : public BaseError +{ + public: + /** @brief Runtime error constructor. + + @param desc_code Index into the error descriptions string table. */ + RuntimeError(unsigned int desc_code) + : BaseError(desc_code, "RuntimeError") + {} + RuntimeError(unsigned int desc_code, char const* error_type) + : BaseError(desc_code, error_type) + {} + virtual ~RuntimeError() throw() {}; +}; // class RuntimeError + + +/// Read error class +class HOKUYOAIST_EXPORT ReadError: public RuntimeError +{ + public: + /** @brief Read error constructor. + + @param desc_code Index into the error descriptions string table. */ + ReadError(unsigned int desc_code) + : RuntimeError(desc_code, "ReadError") + {} +}; // class ReadError + + +/// Write error class +class HOKUYOAIST_EXPORT WriteError: public RuntimeError +{ + public: + /** @brief Write error constructor. + + @param desc_code Index into the error descriptions string table. */ + WriteError(unsigned int desc_code) + : RuntimeError(desc_code, "WriteError") + {} +}; // class WriteError + + +/// Baudrate error class +class HOKUYOAIST_EXPORT BaudrateError: public RuntimeError +{ + public: + /** @brief Baud rate error constructor. + + @param baud The bad baud rate. */ + BaudrateError(unsigned int baud) + : RuntimeError(6, "BaudrateError"), baud_(baud) + {} + BaudrateError(BaudrateError const& rhs) + : RuntimeError(rhs), baud_(rhs.baud()) + {} + + unsigned int baud() const throw() + { return baud_; } + + const char* what() throw(); + + protected: + /** Baud rate that caused the error. */ + unsigned int baud_; +}; // class BaudrateError + + +/// Close error class +class HOKUYOAIST_EXPORT CloseError: public RuntimeError +{ + public: + CloseError() + : RuntimeError(3, "CloseError") + {} +}; // class CloseError + + +/// No destination error class +class HOKUYOAIST_EXPORT NoDestinationError: public RuntimeError +{ + public: + NoDestinationError() + : RuntimeError(11, "NoDestinationError") + {} +}; // class NoDestinationError + + +/// Bad firmware error class +class HOKUYOAIST_EXPORT FirmwareError: public RuntimeError +{ + public: + FirmwareError() + : RuntimeError(23, "FirmwareError") + {} +}; // class FirmwareError + + +/// SCIP version error class +class HOKUYOAIST_EXPORT ScipVersionError: public RuntimeError +{ + public: + ScipVersionError() + : RuntimeError(22, "ScipVersionError") + {} +}; // class ScipVersionError + + +/// Unknown SCIP version error class +class HOKUYOAIST_EXPORT UnknownScipVersionError: public RuntimeError +{ + public: + UnknownScipVersionError() + : RuntimeError(4, "UnknownScipVersionError") + {} +}; // class UnknownScipVersionError + + +/// Unsupported feature error class +class HOKUYOAIST_EXPORT UnsupportedError: public RuntimeError +{ + public: + /** @brief Unsupported error constructor. + + @param desc_code Index into the error descriptions string table. */ + UnsupportedError(unsigned int desc_code) + : RuntimeError(desc_code, "UnsupportedError") + {} +}; // class UnsupportedError + + +/// Bad argument error class +class HOKUYOAIST_EXPORT ArgError: public RuntimeError +{ + public: + /** @brief Argument error constructor. + + @param desc_code Index into the error descriptions string table. */ + ArgError(unsigned int desc_code) + : RuntimeError(desc_code, "ArgError") + {} + ArgError(unsigned int desc_code, char const* error_type) + : RuntimeError(desc_code, error_type) + {} + virtual ~ArgError() throw() {}; +}; // class ArgError + + +/// No data error class +class HOKUYOAIST_EXPORT NoDataError: public RuntimeError +{ + public: + NoDataError() + : RuntimeError(13, "NoDataError") + {} +}; // class NoDataError + + +/// Not a serial connection error class +class HOKUYOAIST_EXPORT NotSerialError: public RuntimeError +{ + public: + NotSerialError() + : RuntimeError(5, "NotSerialError") + {} +}; // class NotSerialError + + +/// Bad index error class +class HOKUYOAIST_EXPORT IndexError: public RuntimeError +{ + public: + IndexError() + : RuntimeError(2, "IndexError") + {} +}; // class IndexError + + +/// Set IP error class +class HOKUYOAIST_EXPORT SetIPError: public RuntimeError +{ + public: + SetIPError() + : RuntimeError(37, "SetIPError") + {} +}; // class SetIPError + + +/// Invalid motor speed error class +class HOKUYOAIST_EXPORT MotorSpeedError: public ArgError +{ + public: + MotorSpeedError() + : ArgError(9, "MotorSpeedError") + {} +}; // class MotorSpeedError + + +/// Bad start step error class +class HOKUYOAIST_EXPORT StartStepError: public ArgError +{ + public: + StartStepError() + : ArgError(14, "StartStepError") + {} +}; // class StartStepError + + +/// Bad end step error class +class HOKUYOAIST_EXPORT EndStepError: public ArgError +{ + public: + EndStepError() + : ArgError(15, "EndStepError") + {} +}; // class EndStepError + + +/// Base protocol error +class HOKUYOAIST_EXPORT ProtocolError: public RuntimeError +{ + public: + /** @brief Protocol error constructor. + + @param desc_code Index into the error descriptions string table. */ + ProtocolError(unsigned int desc_code) + : RuntimeError(desc_code, "ProtocolError") + {} + ProtocolError(unsigned int desc_code, char const* error_type) + : RuntimeError(desc_code, error_type) + {} + virtual ~ProtocolError() throw() {} +}; // class ProtocolError + + +/// Bad checksum error +class HOKUYOAIST_EXPORT ChecksumError: public ProtocolError +{ + public: + /** @brief Checksum error constructor. + + @param expected The expected checksum. + @param calculated The calculated checksum. */ + ChecksumError(int expected, int calculated) + : ProtocolError(24, "ChecksumError"), expected_(expected), + calculated_(calculated) + {} + ChecksumError(ChecksumError const& rhs) + : ProtocolError(rhs), expected_(rhs.expected()), + calculated_(rhs.calculated()) + {} + + virtual int expected() const throw() + { return expected_; } + + virtual int calculated() const throw() + { return calculated_; } + + const char* what() throw(); + + protected: + /** Expected checksum value. */ + int expected_; + /** Calculated checksum value. */ + int calculated_; +}; // class ProtocolError + + +/// Incorrect number of data sets read error +class HOKUYOAIST_EXPORT DataCountError: public ProtocolError +{ + public: + DataCountError() + : ProtocolError(25, "DataCountError") + {} +}; // class DataCountError + + +/// Misplaced line feed error +class HOKUYOAIST_EXPORT MisplacedLineFeedError: public ProtocolError +{ + public: + MisplacedLineFeedError() + : ProtocolError(26, "MisplacedLineFeedError") + {} +}; // class MisplacedLineFeedError + + +/// UnknownLine error +class HOKUYOAIST_EXPORT UnknownLineError: public ProtocolError +{ + public: + /** @brief Unknown line error constructor. + + @param line The mystery line that was not understood. */ + UnknownLineError(char const* const line); + UnknownLineError(UnknownLineError const& rhs); + + virtual char const* const line() const throw() + { return line_; } + + const char* what() throw(); + + protected: + /** The mystery line. */ + char line_[128]; +}; // class UnknownLineError + + +/// Parse error +class HOKUYOAIST_EXPORT ParseError: public ProtocolError +{ + public: + /** @brief Parse error constructor. + + @param line The line that could not be parsed. + @param type The type of line that was expected. */ + ParseError(char const* const line, char const* const type); + ParseError(ParseError const& rhs); + + virtual char const* const line() const throw() + { return line_; } + + virtual char const* const type() const throw() + { return type_; } + + const char* what() throw(); + + protected: + /** The bad line. */ + char line_[128]; + /** The type of line. */ + char type_[16]; +}; // class ParseError + + +/// Missing firmware specification error +class HOKUYOAIST_EXPORT MissingFirmSpecError: public ProtocolError +{ + public: + MissingFirmSpecError() + : ProtocolError(29, "MissingFirmSpecError") + {} +}; // class MissingFirmSpecError + + +/// Bad response error - may be sent in response to any command +class HOKUYOAIST_EXPORT ResponseError: public ProtocolError +{ + public: + /** @brief Response error constructor. + + @param error The two-byte error code received. + @param cmd The command that caused the error. */ + ResponseError(char const* const error, char const* const cmd) + : ProtocolError(30, "ResponseError") + { + error_[0] = error[0]; error_[1] = error[1]; + cmd_[0] = cmd[0]; cmd_[1] = cmd[1]; + } + ResponseError(ResponseError const& rhs) + : ProtocolError(rhs) + { + error_[0] = rhs.error_code()[0]; + error_[1] = rhs.error_code()[1]; + cmd_[0] = rhs.cmd_code()[0]; + cmd_[1] = rhs.cmd_code()[1]; + } + + /// Get the two-byte error code as a non-null-terminated array. + virtual char const* const error_code() const throw() + { return error_; } + + /// Get the two-byte command code as a non-null-terminated array. + virtual char const* const cmd_code() const throw() + { return cmd_; } + + const char* what() throw(); + + protected: + /** Error code as defined in SCIP2 (two bytes). */ + char error_[2]; + /** Command that triggered the error, from SCIP2 (two bytes). */ + char cmd_[2]; +}; // class ResponseError + + +/// Bad response error (SCIP1 version) +class HOKUYOAIST_EXPORT Scip1ResponseError: public ProtocolError +{ + public: + /** @brief Response error constructor. + + @param error The two-byte error code received. + @param cmd The command that caused the error. */ + Scip1ResponseError(char error, char cmd) + : ProtocolError(30, "Scip1ResponseError"), + error_(error), cmd_(cmd) + {} + Scip1ResponseError(Scip1ResponseError const& rhs) + : ProtocolError(rhs), error_(rhs.error_code()), + cmd_(rhs.cmd_code()) + {} + + /// Get the one-byte error code. + virtual char error_code() const throw() + { return error_; } + + /// Get the one-byte command code. + virtual char cmd_code() const throw() + { return cmd_; } + + const char* what() throw(); + + protected: + /** Error code as defined in SCIP2 (two bytes). */ + char error_; + /** Command that triggered the error, from SCIP2 (two bytes). */ + char cmd_; +}; // class Scip1ResponseError + + +/// Command echo error +class HOKUYOAIST_EXPORT CommandEchoError: public ProtocolError +{ + public: + /** @brief Command echo error constructor. + + @param cmd The two-byte command code expected. + @param echo The two-byte command echo received. */ + CommandEchoError(char const* const cmd, char const* const echo) + : ProtocolError(31, "CommandEchoError") + { + cmd_[0] = cmd[0]; cmd_[1] = cmd[1]; + echo_[0] = echo[0]; echo_[1] = echo[1]; + } + CommandEchoError(CommandEchoError const& rhs) + : ProtocolError(rhs) + { + cmd_[0] = rhs.cmd_code()[0]; + cmd_[1] = rhs.cmd_code()[1]; + echo_[0] = rhs.cmd_echo()[0]; + echo_[1] = rhs.cmd_echo()[1]; + } + + /// Get the two-byte command code as a non-null-terminated array. + virtual char const* const cmd_code() const throw() + { return cmd_; } + + /// Get the two-byte command echo as a non-null-terminated array. + virtual char const* const cmd_echo() const throw() + { return echo_; } + + const char* what() throw(); + + protected: + /** Command that triggered the error, from SCIP2 (two bytes). */ + char cmd_[2]; + /** Received echo. */ + char echo_[2]; +}; // class CommandEchoError + + +/// Parameter echo error +class HOKUYOAIST_EXPORT ParamEchoError: public ProtocolError +{ + public: + /** @brief Parameter echo error constructor. + + @param cmd The two-byte command code sent. */ + ParamEchoError(char const* const cmd) + : ProtocolError(32, "ParamEchoError") + { + cmd_[0] = cmd[0]; cmd_[1] = cmd[1]; + } + ParamEchoError(ParamEchoError const& rhs) + : ProtocolError(rhs) + { + cmd_[0] = rhs.cmd_code()[0]; + cmd_[1] = rhs.cmd_code()[1]; + } + + /// Get the two-byte command code as a non-null-terminated array. + virtual char const* const cmd_code() const throw() + { return cmd_; } + + const char* what() throw(); + + protected: + /** Command that triggered the error, from SCIP2 (two bytes). */ + char cmd_[2]; +}; // class ParamEchoError + + +/// Insufficient bytes to calculate checksum error +class HOKUYOAIST_EXPORT InsufficientBytesError: public ProtocolError +{ + public: + /** @brief Insufficient bytes error constructor. + + @param num The number of bytes received. + @param line_length The length of the line. */ + InsufficientBytesError(int num, int line_length) + : ProtocolError(33, "InsufficientBytesError"), + num_(num), line_length_(line_length) + {} + InsufficientBytesError(InsufficientBytesError const& rhs) + : ProtocolError(rhs), num_(rhs.num()), + line_length_(rhs.line_length()) + {} + + virtual int num() const throw() + { return num_; } + + virtual int line_length() const throw() + { return line_length_; } + + const char* what() throw(); + + protected: + /** Number of bytes available. */ + int num_; + /** Length of the line. */ + int line_length_; +}; // class InsufficientBytesError + + +/// Incorrect line length error +class HOKUYOAIST_EXPORT LineLengthError: public ProtocolError +{ + public: + /** @brief Line length error constructor. + + @param length The number of bytes received. + @param expected The expected length of the line. */ + LineLengthError(int length, int expected) + : ProtocolError(34, "LineLengthError"), + length_(length), expected_(expected) + {} + LineLengthError(LineLengthError const& rhs) + : ProtocolError(rhs), length_(rhs.length()), + expected_(rhs.expected()) + {} + + virtual int length() const throw() + { return length_; } + + virtual int expected() const throw() + { return expected_; } + + const char* what() throw(); + + protected: + /** The received line length. */ + int length_; + /** The expected line length. */ + int expected_; +}; // class LineLengthError + +}; // namespace hokuyoaist + +/** @} */ + +#endif // HOKUYO_ERRORS_H__ + diff --git a/include/hokuyoaist/hokuyoaist.h b/include/hokuyoaist/hokuyoaist.h new file mode 100644 index 0000000..df74317 --- /dev/null +++ b/include/hokuyoaist/hokuyoaist.h @@ -0,0 +1,44 @@ +/* HokuyoAIST + * + * Main header file. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef HOKUYOAIST_H__ +#define HOKUYOAIST_H__ + +#include +#include +#include +#include + +// TODO: The line reading code is suffering from age. It is getting bloated and +// complicated, not to mention slow. Reading the range data has been switched +// to an improved method. The rest of the reads need to be moved to this new +// method. Not urgent as only the range data needs to be read really +// efficiently. + +#endif // HOKUYOAIST_H__ + diff --git a/include/hokuyoaist/scan_data.h b/include/hokuyoaist/scan_data.h new file mode 100644 index 0000000..d3d4d8f --- /dev/null +++ b/include/hokuyoaist/scan_data.h @@ -0,0 +1,170 @@ +/* HokuyoAIST + * + * Header file for the scan data object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef SCAN_DATA_H__ +#define SCAN_DATA_H__ + +#if defined(WIN32) + typedef unsigned char uint8_t; + typedef unsigned int uint32_t; + #if defined(HOKUYOAIST_STATIC) + #define HOKUYOAIST_EXPORT + #elif defined(hokuyoaist_EXPORTS) + #define HOKUYOAIST_EXPORT __declspec(dllexport) + #else + #define HOKUYOAIST_EXPORT __declspec(dllimport) + #endif +#else + #include + #define HOKUYOAIST_EXPORT +#endif + +#include + +#include + +/** @ingroup library_hokuyoaist +@{ +*/ + +namespace hokuyoaist +{ + +class Sensor; + +/** @brief Structure to store data returned from the laser scanner. */ +class HOKUYOAIST_EXPORT ScanData +{ + public: + friend class Sensor; + + /// This constructor creates an empty ScanData with no data currently + /// allocated. + ScanData(); + /// This constructor uses a provided data buffer rather than allocating + /// automatically. + /// + /// If the intensity pointer is 0, no data will be provided of that + /// type. + /// + /// @param ranges_buffer A pointer to a data area to store range data + /// in. It is the caller's responsibility to ensure that it is big + /// enough. + /// @param ranges_length The size of the ranges buffer. Used only for + /// copy constructor and similar. + /// @param intensities_buffer A pointer to a data area to store + /// intensity data in. It is the caller's responsibility to ensure that + /// it is big enough. + /// @param intensities_length The size of the intensities buffer. Used + /// only for copy constructor and similar. + ScanData(uint32_t* const ranges_buffer, + unsigned int ranges_length, + uint32_t* const intensities_buffer=0, + unsigned int intensities_length=0); + /// This copy constructor performs a deep copy of present data. + ScanData(ScanData const& rhs); + ~ScanData(); + + /** @brief Return a pointer to array of range readings in millimetres. + + Values less than 20mm indicate an error. Check the error value for the + data to see a probable cause for the error. Most of the time, it will + just be an out-of-range reading. */ + const uint32_t* ranges() const + { return ranges_; } + /// @brief Return a pointer to an array of intensity readings. + const uint32_t* intensities() const + { return intensities_; } + /// @brief Get the number of range samples in the data. + unsigned int ranges_length() const { return ranges_length_; } + /// @brief Get the number of intensity samples in the data. + unsigned int intensities_length() const { return intensities_length_; } + /** @brief Indicates if one or more steps had an error. + + A step's value will be less than 20 if it had an error. Use @ref + error_code_to_string to get a textual representation of the error. */ + bool get_error_status() const { return error_; } + /// @brief Return a string representing the error for the given error + /// code. + std::string error_code_to_string(uint32_t error_code); + /** @brief Get the raw time stamp of the data in milliseconds. + + This value is only available using SCIP version 2). */ + unsigned int laser_time_stamp() const { return laser_time_; } + /** @brief Get the system time stamp of the data in milliseconds. + + This value is only available using SCIP version 2). */ + unsigned long long system_time_stamp() const { return system_time_; } + /// Get the model of the laser that produced this scan. + LaserModel model() const { return model_; } + /// Check if the buffers are being provided instead of automatic. + bool buffers_provided() const { return buffers_provided_; } + + /// @brief Assignment operator. + /// + /// If the rhs has provided buffers, the lhs will not receive the same + /// buffers. Instead, it will copy the data into its own buffers. + /// If the lhs has provided buffers, it is the caller's responsibility + /// to ensure they will be big enough to receive the data from the rhs, + /// except in the case of 0 buffers (no data will be copied for 0 + /// buffers). + ScanData& operator=(ScanData const& rhs); + /** @brief Subscript operator. + + Provides direct access to an element of the range data. */ + uint32_t operator[](unsigned int index); + + /// @brief Format the entire object into a string. + std::string as_string(); + + /// @brief Force the data to clean up. + void clean_up(); + + protected: + uint32_t* ranges_; + uint32_t* intensities_; + unsigned int ranges_length_; + unsigned int intensities_length_; + bool error_; + unsigned int laser_time_; + unsigned long long system_time_; + LaserModel model_; + bool buffers_provided_; + + void allocate_data(unsigned int length, + bool include_intensities = false); + void write_range(unsigned int index, uint32_t value); + void write_intensity(unsigned int index, uint32_t value); +}; // class ScanData + +} // namespace hokuyoaist + +/** @} */ + +#endif // SCAN_DATA_H__ + diff --git a/include/hokuyoaist/sensor.h b/include/hokuyoaist/sensor.h new file mode 100644 index 0000000..342024c --- /dev/null +++ b/include/hokuyoaist/sensor.h @@ -0,0 +1,571 @@ +/* HokuyoAIST + * + * Header file for the sensor object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef SENSOR_H__ +#define SENSOR_H__ + +#include + +#if defined(WIN32) + typedef unsigned char uint8_t; + typedef unsigned int uint32_t; + #if defined(HOKUYOAIST_STATIC) + #define HOKUYOAIST_EXPORT + #elif defined(hokuyoaist_EXPORTS) + #define HOKUYOAIST_EXPORT __declspec(dllexport) + #else + #define HOKUYOAIST_EXPORT __declspec(dllimport) + #endif +#else + #include + #define HOKUYOAIST_EXPORT +#endif + +namespace flexiport +{ + class Port; +} + +/** @ingroup library_hokuyoaist +@{ +*/ + +namespace hokuyoaist +{ + +/// @brief Possible values of the multiecho mode setting. +/// +/// The Tough-URG features multiecho detection capability. To use this, set +/// the sensor to use any mode other than ME_OFF. +/// The sensor can register up to three echos for a single reading. The +/// multiecho mode determines how these are combined into a single value: +/// - ME_FRONT: only the closest reading will be used. +/// - ME_MIDDLE: the middle reading will be used, or the closest +/// reading if there are only two echos. +/// - ME_REAR: the furthest reading will be used. +/// - ME_AVERAGE: the average of all two or three echos will be used. +/// In all cases, if there is only one echo, then this setting has no effect. +enum MultiechoMode +{ + ME_OFF, + ME_FRONT, + ME_MIDDLE, + ME_REAR, + ME_AVERAGE +}; + + +HOKUYOAIST_EXPORT inline char const* multiecho_mode_to_string(MultiechoMode mode) +{ + switch(mode) + { + case ME_OFF: + return "Off"; + case ME_FRONT: + return "Front"; + case ME_MIDDLE: + return "Middle"; + case ME_REAR: + return "Rear"; + case ME_AVERAGE: + return "Average"; + default: + return "Unknown"; + } +} + + +/// Structure to store an IP address. +typedef struct IPAddr +{ + /// First byte + unsigned int first; + /// Second byte + unsigned int second; + /// Third byte + unsigned int third; + /// Fourth byte + unsigned int fourth; +} IPAddr; + + +/** @brief Hokuyo laser scanner class. + +Provides an interface for interacting with a Hokuyo laser scanner using SCIP +protocol version 1 or 2. The FlexiPort library is used to implement the data +communications with the scanner. See its documentation for details on +controlling the connection. + +To use a serial connection, ensure that you do not also have a USB cable +connected, as this will force the scanner into USB mode, preventing the serial +connection from functioning correctly. + +All functions may throw instances of @ref BaseError or its children. +Exceptions from FlexiPort may also occur. */ +class HOKUYOAIST_EXPORT Sensor +{ + public: + Sensor(); + Sensor(std::ostream& err_output); + ~Sensor(); + + /// @brief Open the laser scanner and begin scanning. + void open(std::string port_options); + + /** @brief Open the laser scanner and begin scanning, probing the baud + rate. + + If the port is a serial connection and communication with the laser + fails at the given baud rate, the alternative baud rates supported by + the device are tried (see @ref set_baud for these) in order from fastest + to slowest. + + @return The baud rate at which connection with the laser succeeded, or + 0 for non-serial connections. */ + unsigned int open_with_probing(std::string port_options); + + /// @brief Close the connection to the laser scanner. + void close(); + + /// @brief Checks if the connection to the laser scanner is open. + bool is_open() const; + + /// @brief Switch the laser scanner on or off. + void set_power(bool on); + + /** @brief Change the baud rate when using a serial connection. + + Valid rates are 19.2Kbps, 38.4Kbps, 57.6Kbps, 115.2Kbps, 250.0Kbps, + 500.0Kbps, 750.0Kbps (dependent on those available in FlexiPort). */ + void set_baud(unsigned int baud); + + /** @brief Change the IP address information. + + This function only works on devices providing an ethernet connection. + + Once this function has successfully completed, the laser must be + restarted for it to take effect. */ + void set_ip(IPAddr const& addr, IPAddr const& subnet, + IPAddr const& gateway); + + /** @brief Reset the laser scanner to its default settings. + + Not available with the SCIP v1 protocol. */ + void reset(); + + /** @brief Reset everything except motor and serial speed. + + Requires SCIP v2.1 or higher. */ + void semi_reset(); + + /** @brief Set the speed at which the scanner's sensor spins. + + Set the speed to 0 to have it reset to the default value, and 99 to + reset it to the initial (startup) value. Values between 1 and 10 + specify a ratio of the default speed. The speeds in revolutions per + minute that these correspond to will depend on the scanner model. For + example, for a URG-04LX, they are (from 1 to 10) 594, 588, 576, 570, + 564, 558, 552, 546, and 540 rpm. + + Not available with the SCIP v1 protocol. */ + void set_motor_speed(unsigned int speed); + + /** @brief Switch the scanner between normal and high sensitivity + modes. */ + void set_high_sensitivity(bool on); + + /** @brief Get various information about the scanner. + + Much of the information is not available with the SCIP v1 protocol. */ + void get_sensor_info(SensorInfo& info); + + /** @brief Get the value of the scanner's clock in milliseconds. + + Not available with the SCIP v1 protocol. */ + unsigned long long get_time(); + + /** @brief Get the raw value of the scanner's clock in milliseconds. + + Not available with the SCIP v1 protocol. */ + unsigned int get_raw_time(); + + /** @brief Calibrate the time offset between the laser and computer. + + This function performs several checks of the communications between the + laser and the computer. Its goal is to calculate the communications lag + between the two devices, and so determine as accurately as possible + what the offset is from the computer's clock to the laser's clock. This + offset is necessary to calculate the time stamp of scans in terms of + the computer's clock. + + Because this function sends a lot of data to and from the laser, it + may take some time to complete. It will never be called automatically; + until it is called, the offset defaults to zero. + + Note that the laser's clock is a 24-bit millisecond timer. It wraps + approximately every 9.5 hours. This class will detect wrapped time + stamps and adjust them accordingly. + + @param skew_sleep_time Whether to approximate the laser clock skew. + This requires sleeping for the specified time, then calibrating again. + The results of the two calibrations are used to approximate the clock + skew as a straight line between the two points. This is very + approximate; in order to overcome noise in the signal you will need to + calibrate over a period of up to several minutes. Set this to 0 to not + approximate the skew. + @param samples The number of samples to use in calculating latencies. + @return The calculated offset in nanoseconds. + */ + + long long calibrate_time(unsigned int skew_sleep_time=0, + unsigned int samples=10); + + /// Retrieve the calculated time offset (0 if not calibrated). + long long time_offset() const { return time_offset_; } + + /// Set the time offset (if the calculated value is bad). + void set_time_offset(long long time_offset) + { time_offset_ = time_offset; } + /// Retrieve the current clock drift rate (0 if not set). + float drift_rate() const { return time_drift_rate_; } + /** Set the current clock drift rate. + + The drift rate is used when correcting laser time stamps to computer + time. It is factored into the equation as: + Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha) + where Tc is the computer time and Tl is the laser time. See @ref + set_skew_alpha for the values of alpha and beta. + + If the drift rate is zero, it means the laser's clock does not drift. + This is extremely unlikely, but the laser's drift may not matter for + your application. + + Drift should usually be provided by the manufacturer. If it is not, + you can calculate it by calibrating the laser many times over a long + period and looking at the change in the calculated offset. */ + void set_drift_rate(float drift_rate) + { time_drift_rate_ = drift_rate; } + + /// Get the calculated skew line slope (default: 0). + float skew_alpha() const { return time_skew_alpha_; } + /** Set a skew line slope value. + + The skew is used when correcting laser time stamps to computer time. + It is factored into the equation as: + Tc = ((1 - drift)Tl + offset + beta) / (1 - alpha) + where Tc is the computer time and Tl is the laser time. See @ref + drift_rate for the value of the drift. Beta, the line's y crossing, is + assumed to be 0 since we always use a two-point line fit instead of + something like least squares. + + The effect of this is to cancel out the skew caused by the different + frequencies of the computer clock and the laser clock. */ + void set_skew_alpha(float alpha) { time_skew_alpha_ = alpha; } + + /** @brief Get the latest scan data from the scanner. + + This function requires a pointer to a @ref ScanData object. It will + allocate space in this object as necessary for storing range data. If + the passed-in @ref ScanData object already has the correct quantity + of space to store the range data, it will not be re-allocated. If it + does not have any space, it will be allocated. If it has space, but it + is the wrong size, it will be re-allocated. This means you can + repeatedly send the same @ref ScanData object without having to worry + about allocating its data, whether it will change or not, while also + avoiding excessive allocations. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @param start_step The first step to get ranges from. Set to -1 for the + first scannable step. + @param end_step The last step to get ranges from. Set to -1 for the last + scannable step. + @return The number of range readings read into data. */ + unsigned int get_ranges(ScanData& data, int start_step = -1, + int end_step = -1, unsigned int cluster_count = 1); + + /** @brief Get the latest scan data from the scanner. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param start_angle The angle to get range readings from. Exclusive; if + this falls between two steps the step inside the angle will be + returned, but the step outside won't. + @param end_angle The angle to get range readings to. Exclusive; if this + falls between two steps the step inside the angle will be returned, but + the step outside won't. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @return The number of range readings read into data. */ + unsigned int get_ranges_by_angle(ScanData& data, double start_angle, + double end_angle, unsigned int cluster_count = 1); + + /** @brief Get the latest scan data from the scanner with intensities. + + This function requires a pointer to a @ref ScanData object. It will + allocate space in this object as necessary for storing range data. If + the passed-in @ref ScanData object already has the correct quantity + of space to store the range data, it will not be re-allocated. If it + does not have any space, it will be allocated. If it has space, but it + is the wrong size, it will be re-allocated. This means you can + repeatedly send the same @ref ScanData object without having to worry + about allocating its data, whether it will change or not, while also + avoiding excessive allocations. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @param start_step The first step to get ranges from. Set to -1 for the + first scannable step. + @param end_step The last step to get ranges from. Set to -1 for the last + scannable step. + @return The number of range readings read into data. */ + unsigned int get_ranges_intensities(ScanData& data, + int start_step = -1, int end_step = -1, + unsigned int cluster_count = 1); + + /** @brief Get the latest scan data from the scanner with intensities. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param start_angle The angle to get range readings from. Exclusive; if + this falls between two steps the step inside the angle will be + returned, but the step outside won't. + @param end_angle The angle to get range readings to. Exclusive; if this + falls between two steps the step inside the angle will be returned, but + the step outside won't. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @return The number of range readings read into data. */ + unsigned int get_ranges_intensities_by_angle(ScanData& data, + double start_angle, double end_angle, + unsigned int cluster_count = 1); + + /** @brief Get a new scan from the scanner. + + Unlike @ref get_ranges, which returns the most recent scan the scanner + took, this function will request a new scan. This means it will wait + while the scanner performs the scan, which means the rate at which + scans can be retrieved using this function is less than with @ref + get_ranges. Otherwise behaves identicallty to @ref get_ranges. + + Not available with the SCIP v1 protocol. + + @note The command used to retrieve a fresh scan is also used for the + continuous scanning mode (not yet supported by this library). After + completing a scan, it will turn the laser off (in anticipation of + another continuous scan command being sent, which will automatically + turn the laser back on again). If you want to mix @ref get_new_ranges and + @ref get_ranges, you will need to turn the laser on after each call to + @ref get_new_ranges. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @param start_step The first step to get ranges from. Set to -1 for the + first scannable step. + @param end_step The last step to get ranges from. Set to -1 for the last + scannable step. + @return The number of range readings read into data. */ + unsigned int get_new_ranges(ScanData& data, int start_step = -1, + int end_step = -1, unsigned int cluster_count = 1); + + /** @brief Get a new scan from the scanner. + + Not available with the SCIP v1 protocol. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param start_angle The angle to get range readings from. Exclusive; if + this falls between two steps the step inside the angle will be + returned, but the step outside won't. + @param end_angle The angle to get range readings to. Exclusive; if this + falls between two steps the step inside the angle will be returned, but + the step outside won't. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @return The number of range readings read into data. */ + unsigned int get_new_ranges_by_angle(ScanData& data, + double start_angle, double end_angle, + unsigned int cluster_count = 1); + + /** @brief Get a new scan from the scanner with intensity data. + + Unlike @ref get_ranges, which returns the most recent scan the scanner + took, this function will request a new scan. This means it will wait + while the scanner performs the scan. Otherwise behaves identicallty to + @ref get_ranges. + + Not available with the SCIP v1 protocol. + + @note The command used to retrieve a fresh scan is also used for the + continuous scanning mode (not yet supported by this library). After + completing a scan, it will turn the laser off (in anticipation of + another continuous scan command being sent, which will automatically + turn the laser back on again). If you want to mix @ref get_new_ranges and + @ref get_ranges, you will need to turn the laser on after each call to + @ref get_new_ranges. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @param start_step The first step to get ranges from. Set to -1 for the + first scannable step. + @param end_step The last step to get ranges from. Set to -1 for the last + scannable step. + @return The number of range readings read into data. */ + unsigned int get_new_ranges_intensities(ScanData& data, + int start_step = -1, int end_step = -1, + unsigned int cluster_count = 1); + + /** @brief Get a new scan from the scanner with intensity data. + + Not available with the SCIP v1 protocol. + + @param data Pointer to a @ref ScanData object to store the range + readings in. + @param start_angle The angle to get range readings from. Exclusive; if + this falls between two steps the step inside the angle will be + returned, but the step outside won't. + @param end_angle The angle to get range readings to. Exclusive; if this + falls between two steps the step inside the angle will be returned, but + the step outside won't. + @param cluster_count The number of readings to cluster together into a + single reading. The minimum value from a cluster is returned as the + range for that cluster. + @return The number of range readings read into data. */ + unsigned int get_new_ranges_intensities_by_angle(ScanData& data, + double start_angle, double end_angle, + unsigned int cluster_count = 1); + + /// @brief Return the major version of the SCIP protocol in use. + uint8_t scip_version() const { return scip_version_; } + + /** @brief Turns on and off printing of verbose operating information + to stderr. Default is off. */ + void set_verbose(bool verbose) { verbose_ = verbose; } + + /** @brief Enables/disables ignoring unknown lines in sensor info + messages. Default is off. */ + void ignore_unknowns(bool ignore) { ignore_unknowns_ = ignore; } + + /** @brief Set the multi-echo mode to use. Default is ME_OFF. */ + void set_multiecho_mode(MultiechoMode mode) { multiecho_mode_ = mode; } + + /// @brief A convenience function to convert a step index to an angle. + double step_to_angle(unsigned int step); + /** @brief A convenience function to convert an angle to a step + (rounded towards the front). */ + unsigned int angle_to_step(double angle); + + private: + flexiport::Port* port_; + std::ostream& err_output_; + + uint8_t scip_version_; + LaserModel model_; + bool verbose_, enable_checksum_workaround_, + ignore_unknowns_; + MultiechoMode multiecho_mode_; + double min_angle_, max_angle_, resolution_; + int first_step_, last_step_, front_step_; + unsigned int max_range_; + /// The time between two points in a scan, in milliseconds. + unsigned int time_resolution_; + /// The offset from the laser's clock to the computer's clock in + /// nanoseconds. + long long time_offset_; + /// The previous received timestamp from the laser, in laser time and + /// in milliseconds. + unsigned int last_timestamp_; + /// The number of times the laser's clock has wrapped. + unsigned int wrap_count_; + /// The drift rate of the laser's clock + float time_drift_rate_; + /// The clock skew alpha value. + float time_skew_alpha_; + + void clear_read_buffer(); + int read_line(char* buffer, int expected_length=-1); + int read_line_with_check(char* buffer, int expected_length=-1, + bool has_semicolon=false); + bool read_data_block(char* buffer, int& block_size); + void skip_lines(int count); + int send_command(char const* cmd, char const* param, int param_length, + char const* extra_ok); + + void enter_timing_mode(); + void leave_timing_mode(); + /// Get the laser time in milliseconds. + unsigned int get_timing_mode_time(unsigned long long* reception_time=0); + /// Get the computer time in nanoseconds. + unsigned long long get_computer_time(); + /// Adjust a wrapped laser timestamp, in milliseconds. + unsigned int wrap_timestamp(unsigned int timestamp); + /// Offset a laser timestamp, in milliseconds, into computer time, in + /// nanoseconds. + unsigned long long offset_timestamp(unsigned int timestamp); + /// Convert a step value into its time offset from the start of a scan, + /// in milliseconds. + unsigned int step_to_time_offset(int start_step); + + void find_model(char const* buffer); + void get_and_set_scip_version(); + void get_defaults(); + void process_vv_line(char const* buffer, SensorInfo& info); + void process_pp_line(char const* buffer, SensorInfo& info); + void process_ii_line(char const* buffer, SensorInfo& info); + + uint32_t process_echo_buffer(int const* buffer, int num_echos); + void read_2_byte_range_data(ScanData& data, unsigned int num_steps); + void read_3_byte_range_data(ScanData& data, unsigned int num_steps); + void read_3_byte_range_and_intensity_data(ScanData& data, + unsigned int num_steps); + + int confirm_checksum(char const* buffer, int length, + int expected_sum); +}; // class Sensor + +} // namespace hokuyoaist + +/** @} */ + +#endif // SENSOR_H__ + diff --git a/include/hokuyoaist/sensor_info.h b/include/hokuyoaist/sensor_info.h new file mode 100644 index 0000000..cee5d9a --- /dev/null +++ b/include/hokuyoaist/sensor_info.h @@ -0,0 +1,228 @@ +/* HokuyoAIST + * + * Header file for the sensor information object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef SENSOR_INFO_H__ +#define SENSOR_INFO_H__ + +#if defined(WIN32) + typedef unsigned char uint8_t; + typedef unsigned int uint32_t; + #if defined(HOKUYOAIST_STATIC) + #define HOKUYOAIST_EXPORT + #elif defined(hokuyoaist_EXPORTS) + #define HOKUYOAIST_EXPORT __declspec(dllexport) + #else + #define HOKUYOAIST_EXPORT __declspec(dllimport) + #endif +#else + #include + #define HOKUYOAIST_EXPORT +#endif + +#include +#include + +/** @ingroup library_hokuyoaist +@{ +*/ + +namespace hokuyoaist +{ + +/// Laser models +enum LaserModel +{ + MODEL_URG04LX, // Classic-URG + MODEL_UBG04LXF01, // Rapid-URG + MODEL_UHG08LX, // Hi-URG + MODEL_UTM30LX, // Top-URG + MODEL_UXM30LXE, // Tough-URG + MODEL_UNKNOWN +}; + + +HOKUYOAIST_EXPORT inline char const* model_to_string(LaserModel model) +{ + switch(model) + { + case MODEL_URG04LX: + return "URG-04LX"; + case MODEL_UBG04LXF01: + return "UBG-04LX-F01"; + case MODEL_UHG08LX: + return "UHG-08LX"; + case MODEL_UTM30LX: + return "UTM-30LX"; + case MODEL_UXM30LXE: + return "UXM-30LX-E"; + default: + return "Unknown model"; + } +} + + +HOKUYOAIST_EXPORT inline LaserModel string_to_model(char const* model) +{ + if(strncmp(model, "URG-04LX", 8) == 0) + return MODEL_URG04LX; + else if(strncmp(model, "UBG-04LX-F01", 8) == 0) + return MODEL_UBG04LXF01; + else if(strncmp(model, "UHG-08LX", 8) == 0) + return MODEL_UHG08LX; + else if(strncmp(model, "UTM-30LX", 8) == 0) + return MODEL_UTM30LX; + else if(strncmp(model, "UXM-30LX-E", 8) == 0) + return MODEL_UXM30LXE; + else + return MODEL_UNKNOWN; +} + + +/// Sensor direction of rotation +enum RotationDirection +{ + CLOCKWISE, + COUNTERCLOCKWISE +}; + + +HOKUYOAIST_EXPORT inline char const* rot_dir_to_string(RotationDirection dir) +{ + switch(dir) + { + case CLOCKWISE: + return "Clockwise"; + case COUNTERCLOCKWISE: + return "Counter-clockwise"; + default: + return "Unknown"; + } +} + + +// Forward declaration +class Sensor; + + +/** @brief Sensor information. + +Returned from a call to @ref Sensor::get_sensor_info. Contains various +information about the laser scanner such as firmware version and maximum +possible range. */ +class HOKUYOAIST_EXPORT SensorInfo +{ + public: + friend class Sensor; + + SensorInfo(); + SensorInfo(SensorInfo const& rhs); + + /// @brief Assignment operator. + SensorInfo& operator=(SensorInfo const& rhs); + + /// @brief Format the entire object into a string. + std::string as_string(); + + // Version details. + /// Vendor name. + std::string vendor; + /// Product name. + std::string product; + /// Firmware version. + std::string firmware; + /// Protocol version in use. + std::string protocol; + /// Serial number of this device. + std::string serial; + + // Specification details. + /// Sensor model number. + std::string model; + /// Minimum detectable range (mm). + unsigned int min_range; + /// Maximum detectable range (mm). + unsigned int max_range; + /// Number of steps in a 360-degree scan. + unsigned int steps; + /// First scanable step of a full scan. + unsigned int first_step; + /// Last scanable step of a full scan. + unsigned int last_step; + /// Step number that points forward (typically the centre of a full + /// scan). + unsigned int front_step; + /// Standard motor speed (rpm). + unsigned int standard_speed; + /// Rotation direction. + RotationDirection rot_dir; + + // Status details. + /// Operational status - illuminated or not. + bool power; + /// Current motor speed (rpm). + unsigned int speed; + /// Speed level (0 for default) + unsigned short speed_level; + /// Measurement state. + std::string measure_state; + /// Baud rate. + unsigned int baud; + /// Current sensor time (s). + unsigned int time; + /// Diagnostic status string. + std::string sensor_diagnostic; + + // Calculated details + /// Minimum possible scan angle (radians). Scans go anti-clockwise with + /// negative angles on the right. + double min_angle; + /// Maximum possible scan angle (radians). Scans go anti-clockwise with + /// negative angles on the right. + double max_angle; + /// Angle between two scan points (radians). + double resolution; + /// Time between two scan points (milliseconds). + double time_resolution; + /// Total number of steps in a full scan (lastStep - firstStep). + unsigned int scanable_steps; + /// Absolute maximum commandable step. + unsigned int max_step; + /// Detected model of the laser. + LaserModel detected_model; + + private: + void set_defaults(); + void calculate_values(); +}; // class SensorInfo + +}; // namespace hokuyoaist + +/** @} */ + +#endif // SENSOR_INFO_H__ + diff --git a/include/hokuyoaist/utils.h b/include/hokuyoaist/utils.h new file mode 100644 index 0000000..5f5454d --- /dev/null +++ b/include/hokuyoaist/utils.h @@ -0,0 +1,95 @@ +/* HokuyoAIST + * + * Header file for various utilities. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#ifndef UTILS_H__ +#define UTILS_H__ + +#include +#include +#include +#include +#include +#include + +#if defined(WIN32) + typedef unsigned char uint8_t; + typedef unsigned int uint32_t; + #if defined(HOKUYOAIST_STATIC) + #define HOKUYOAIST_EXPORT + #elif defined(hokuyoaist_EXPORTS) + #define HOKUYOAIST_EXPORT __declspec(dllexport) + #else + #define HOKUYOAIST_EXPORT __declspec(dllimport) + #endif +#else + #include + #define HOKUYOAIST_EXPORT +#endif + +/** @ingroup library_hokuyoaist +@{ +*/ + +namespace hokuyoaist +{ + +#ifndef M_PI + double const M_PI = 3.14159265358979323846; +#endif +// Convert radians to degrees +#ifndef RTOD + inline double RTOD(double rad) + { + return rad * 180.0 / M_PI; + } +#endif +// Convert degrees to radians +#ifndef DTOR + inline double DTOR(double deg) + { + return deg * M_PI / 180.0; + } +#endif + + +/// Find the median value of a std::vector. +template +inline T median(std::vector& v) +{ + typename std::vector::iterator first(v.begin()); + typename std::vector::iterator median(first + (v.end() - first) / 2); + std::nth_element(first, median, v.end()); + return *median; +} + +} // namespace hokuyoaist + +/** @} */ + +#endif // UTILS_H__ + diff --git a/python/.svn/all-wcprops b/python/.svn/all-wcprops new file mode 100644 index 0000000..e948d3c --- /dev/null +++ b/python/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 66 +/svnroot/gearbox/!svn/ver/522/gearbox/trunk/src/hokuyo_aist/python +END +hokuyo_aist.cpp +K 25 +svn:wc:ra_dav:version-url +V 82 +/svnroot/gearbox/!svn/ver/522/gearbox/trunk/src/hokuyo_aist/python/hokuyo_aist.cpp +END +CMakeLists.txt +K 25 +svn:wc:ra_dav:version-url +V 81 +/svnroot/gearbox/!svn/ver/532/gearbox/trunk/src/hokuyo_aist/python/CMakeLists.txt +END diff --git a/python/.svn/entries b/python/.svn/entries new file mode 100644 index 0000000..677dbd0 --- /dev/null +++ b/python/.svn/entries @@ -0,0 +1,99 @@ +10 + +dir +531 +https://gearbox.svn.sf.net/svnroot/gearbox/gearbox/trunk/src/hokuyo_aist/python +https://gearbox.svn.sf.net/svnroot/gearbox + + + +2010-10-28T09:08:54.881570Z +522 +gbiggs + + + + + + + + + + + + + + +4b7f92db-2845-0410-95f1-b1f669fef64b + +test +dir + +hokuyo_aist.cpp +file + + + + +2010-10-28T08:58:04.000000Z +0c199216c6ab121df8c823c9dde2ce6e +2010-10-28T09:08:54.881570Z +522 +gbiggs + + + + + + + + + + + + + + + + + + + + + +8111 + +CMakeLists.txt +file +532 + + + +2011-06-10T01:02:14.000000Z +e40fb3f51bcc4ba01f3573412c4de3a8 +2011-06-10T01:04:14.671810Z +532 +gbiggs + + + + + + + + + + + + + + + + + + + + + +2865 + diff --git a/python/.svn/text-base/CMakeLists.txt.svn-base b/python/.svn/text-base/CMakeLists.txt.svn-base new file mode 100644 index 0000000..31c60cb --- /dev/null +++ b/python/.svn/text-base/CMakeLists.txt.svn-base @@ -0,0 +1,65 @@ +# Find the Python libraries and headers +FIND_PACKAGE (PythonLibs) +if(NOT PYTHON_LIBRARIES) + MESSAGE (STATUS "Python libaries not found. Cannot build Python bindings for Hokuyo_aist.") +endif(NOT PYTHON_LIBRARIES) + +# Find Boost::Python +# There is a new, much better, FindBoost.cmake since 2.6 +if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" OFF) + MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS) + + SET (BOOST_COMPONENTS python) + FIND_PACKAGE (Boost COMPONENTS ${BOOST_COMPONENTS}) + if(Boost_FOUND) + INCLUDE_DIRECTORIES (${Boost_INCLUDE_DIR}) + LINK_DIRECTORIES (${Boost_LIBRARY_DIRS}) + + if(Boost_PYTHON_FOUND) + set(boostPythonLib ${Boost_PYTHON_LIBRARY}) + else(Boost_PYTHON_FOUND) + MESSAGE (STATUS + "Boost::Python library was not found. Cannot build Python bindings for Hokuyo_aist.") + endif(Boost_PYTHON_FOUND) + else(Boost_FOUND) + MESSAGE (STATUS + "Boost libraries were not found. Cannot build Python bindings for Hokuyo_aist.") + endif(Boost_FOUND) +else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + FIND_PACKAGE (Boost) + if(Boost_FOUND) + # For 2.4, assume that if boost is found then boost::python is present + OPTION (Boost_USE_MULTITHREAD "Use the multithreaded versions of the Boost libraries" ON) + MARK_AS_ADVANCED (Boost_USE_MULTITHREAD) + if(Boost_USE_MULTITHREAD) + SET (BOOST_LIB_SUFFIX "-mt" CACHE STRING "Boost library name suffix") + else(Boost_USE_MULTITHREAD) + SET (BOOST_LIB_SUFFIX "" CACHE STRING "Boost library name suffix") + endif(Boost_USE_MULTITHREAD) + MARK_AS_ADVANCED (BOOST_LIB_SUFFIX) + + SET (boostPythonLib boost_python${BOOST_LIB_SUFFIX}) + INCLUDE_DIRECTORIES (${Boost_INCLUDE_DIRS}) + LINK_DIRECTORIES (${Boost_LIBRARY_DIRS}) + else(Boost_FOUND) + MESSAGE (STATUS + "Boost libraries were not found. Cannot build Python bindings for Hokuyo_aist.") + endif(Boost_FOUND) +endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + +if(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) + MESSAGE (STATUS "Hokuyo_aist Python bindings will be built.") + SET (srcs hokuyo_aist.cpp) + + SET (pyModuleTarget hokuyo_aist_py) + INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR}/src/hokuyo_aist ${PYTHON_INCLUDE_PATH}) + add_library(${pyModuleTarget} MODULE ${srcs}) + TARGET_LINK_LIBRARIES (${pyModuleTarget} hokuyo_aist ${boostPythonLib} ${PYTHON_LIBRARIES}) + SET_TARGET_PROPERTIES (${pyModuleTarget} PROPERTIES PREFIX "" OUTPUT_NAME "hokuyo_aist") + install(TARGETS ${pyModuleTarget} LIBRARY DESTINATION lib/python/site-packages) + + ADD_SUBDIRECTORY (test) +ELSEIF(NOT GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) + MESSAGE (STATUS "Hokuyo_aist Python bindings will not be built - must build hokuyo_aist as a shared library.") +endif(PYTHON_LIBRARIES AND Boost_FOUND AND GBX_DEFAULT_LIB_TYPE STREQUAL SHARED) diff --git a/python/.svn/text-base/hokuyo_aist.cpp.svn-base b/python/.svn/text-base/hokuyo_aist.cpp.svn-base new file mode 100644 index 0000000..bbecf4c --- /dev/null +++ b/python/.svn/text-base/hokuyo_aist.cpp.svn-base @@ -0,0 +1,221 @@ +#include +using namespace hokuyo_aist; + +#include +#include + +#include + +class BaseErrorWrap + : public BaseError, public boost::python::wrapper +{ + public: + BaseErrorWrap(unsigned int desc_code, char const* error_type) + : BaseError(desc_code, error_type) + {} + + BaseErrorWrap(unsigned int desc_code, std::string error_type) + : BaseError(desc_code, error_type.c_str()) + {} + + unsigned int desc_code() const throw() + { + if (boost::python::override f = get_override("desc_code")) + { + return f(); + } + return BaseError::desc_code(); + } + unsigned int default_desc_code() const throw() + { + return BaseError::desc_code(); + } + + char const* error_type() const throw() + { + if (boost::python::override f = get_override("error_type")) + { + return f(); + } + return BaseError::error_type(); + } + char const* default_error_type() const throw() + { + return BaseError::error_type(); + } + + const char* what() throw() + { + if (boost::python::override f = get_override("what")) + { + return f(); + } + return BaseError::what(); + } + const char* default_what() throw() + { + return BaseError::what(); + } +}; + + +class ScanDataWrap : public ScanData, public boost::python::wrapper +{ + public: + ScanDataWrap(void) + : ScanData() + {} + ScanDataWrap(ScanDataWrap const& rhs) + : ScanData(rhs) + {} + + uint32_t range(unsigned int index) + { return ranges_[index]; } + /*unsigned int ranges_length() const + { + if (boost::python::override f = get_override("ranges_length")) + { + return f(); + } + return ScanData::ranges_length(); + }*/ + + uint32_t intensity(unsigned int index) + { return intensities_[index]; } + /*unsigned int intensities_length() const + { + if (boost::python::override f = get_override("intensities_length")) + { + return f(); + } + return ScanData::intensities_length(); + }*/ + + /*bool get_error_status() const + { + if (boost::python::override f = get_override("get_error_status")) + { + return f(); + } + return ScanData::get_error_status(); + } + + std::string error_code_to_string(uint32_t error_code) + { + if (boost::python::override f = get_override("error_code_to_string")) + { + return f(error_code); + } + return ScanData::error_code_to_string(error_code); + } + + unsigned int laser_time_stamp() + { + if (boost::python::override f = get_override("laser_time_stamp")) + { + return f(); + } + return ScanData::laser_time_stamp(); + } + + unsigned int system_time_stamp() + { + if (boost::python::override f = get_override("system_time_stamp")) + { + return f(); + } + return ScanData::system_time_stamp(); + }*/ +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads1, + get_ranges, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads2, + get_ranges_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads3, + get_ranges_intensities, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads4, + get_ranges_intensities_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads5, + get_new_ranges, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads6, + get_new_ranges_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads7, + get_new_ranges_intensities, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads8, + get_new_ranges_intensities_by_angle, 3, 4) + +BOOST_PYTHON_MODULE(hokuyo_aist) +{ + using namespace boost::python; + + class_("BaseError", init()) + .def("desc_code", &BaseError::desc_code, &BaseErrorWrap::default_desc_code) + .def("error_type", &BaseError::error_type, &BaseErrorWrap::default_error_type) + .def("what", &BaseError::what, &BaseErrorWrap::default_what) + ; + + class_("ScanData") + // TODO: write a wrapper function to copy the data into a python array, because this doesn't work +// .def("ranges", &ScanData::ranges, return_value_policy (), with_custodian_and_ward_postcall<1, 0> ()) +// .def("intensities", &ScanData::intensities, return_value_policy (), with_custodian_and_ward_postcall<1, 0> ()) + .def("range", &ScanDataWrap::range) + .def("intensity", &ScanDataWrap::intensity) + .def("ranges_length", &ScanData::ranges_length) + .def("intensities_length", &ScanData::intensities_length) + .def("get_error_status", &ScanData::get_error_status) + .def("error_code_to_string", &ScanData::error_code_to_string) + .def("laser_time_stamp", &ScanData::laser_time_stamp) + .def("system_time_stamp", &ScanData::system_time_stamp) + .def("model", &ScanData::model) + .def("buffers_provided", &ScanData::buffers_provided) + .def("as_string", &ScanData::as_string) + .def("clean_up", &ScanData::clean_up) + ; + + class_("Sensor") + .def("open", &Sensor::open) + .def("open_with_probing", &Sensor::open_with_probing) + .def("close", &Sensor::close) + .def("is_open", &Sensor::is_open) + .def("set_power", &Sensor::set_power) + .def("set_baud", &Sensor::set_baud) + .def("set_ip", &Sensor::set_ip) + .def("reset", &Sensor::reset) + .def("semi_reset", &Sensor::semi_reset) + .def("set_motor_speed", &Sensor::set_motor_speed) + .def("set_high_sensitivity", &Sensor::set_high_sensitivity) + .def("get_sensor_info", &Sensor::get_sensor_info) + .def("get_time", &Sensor::get_time) + .def("get_raw_time", &Sensor::get_raw_time) + .def("calibrate_time", &Sensor::calibrate_time) + .def("time_offset", &Sensor::time_offset) + .def("set_time_offset", &Sensor::set_time_offset) + .def("drift_rate", &Sensor::drift_rate) + .def("set_drift_rate", &Sensor::set_drift_rate) + .def("skew_alpha", &Sensor::skew_alpha) + .def("set_skew_alpha", &Sensor::set_skew_alpha) + .def("get_ranges", &Sensor::get_ranges, sensor_overloads1()) + .def("get_ranges_by_angle", + &Sensor::get_ranges_by_angle, sensor_overloads2()) + .def("get_ranges_intensities", + &Sensor::get_ranges_intensities, sensor_overloads3()) + .def("get_ranges_intensities_by_angle", + &Sensor::get_ranges_intensities_by_angle, sensor_overloads4()) + .def("get_new_ranges", + &Sensor::get_new_ranges, sensor_overloads5()) + .def("get_new_ranges_by_angle", + &Sensor::get_new_ranges_by_angle, sensor_overloads6()) + .def("get_new_ranges_intensities", + &Sensor::get_new_ranges_intensities, sensor_overloads7()) + .def("get_new_ranges_intensities_by_angle", + &Sensor::get_new_ranges_intensities_by_angle, + sensor_overloads8()) + .def("scip_version", &Sensor::scip_version) + .def("set_verbose", &Sensor::set_verbose) + .def("ignore_unknowns", &Sensor::ignore_unknowns) + .def("set_multiecho_mode", &Sensor::set_multiecho_mode) + .def("step_to_angle", &Sensor::step_to_angle) + .def("angle_to_step", &Sensor::angle_to_step) + ; +} diff --git a/python/CMakeLists.txt b/python/CMakeLists.txt new file mode 100644 index 0000000..a78ef53 --- /dev/null +++ b/python/CMakeLists.txt @@ -0,0 +1,66 @@ +# Find the Python libraries and headers +FIND_PACKAGE (PythonLibs) +if(NOT PYTHON_LIBRARIES) + MESSAGE (STATUS "Python libaries not found. Cannot build Python bindings for HokuyoAIST.") +endif(NOT PYTHON_LIBRARIES) + +# Find Boost::Python +# There is a new, much better, FindBoost.cmake since 2.6 +if(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + OPTION (Boost_USE_STATIC_LIBS "Use the static versions of the Boost libraries" OFF) + MARK_AS_ADVANCED (Boost_USE_STATIC_LIBS) + + SET (BOOST_COMPONENTS python) + FIND_PACKAGE (Boost COMPONENTS ${BOOST_COMPONENTS}) + if(Boost_FOUND) + INCLUDE_DIRECTORIES (${Boost_INCLUDE_DIR}) + LINK_DIRECTORIES (${Boost_LIBRARY_DIRS}) + + if(Boost_PYTHON_FOUND) + set(boostPythonLib ${Boost_PYTHON_LIBRARY}) + else(Boost_PYTHON_FOUND) + MESSAGE (STATUS + "Boost::Python library was not found. Cannot build Python bindings for HokuyoAIST.") + endif(Boost_PYTHON_FOUND) + else(Boost_FOUND) + MESSAGE (STATUS + "Boost libraries were not found. Cannot build Python bindings for HokuyoAIST.") + endif(Boost_FOUND) +else(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + FIND_PACKAGE (Boost) + if(Boost_FOUND) + # For 2.4, assume that if boost is found then boost::python is present + OPTION (Boost_USE_MULTITHREAD "Use the multithreaded versions of the Boost libraries" ON) + MARK_AS_ADVANCED (Boost_USE_MULTITHREAD) + if(Boost_USE_MULTITHREAD) + SET (BOOST_LIB_SUFFIX "-mt" CACHE STRING "Boost library name suffix") + else(Boost_USE_MULTITHREAD) + SET (BOOST_LIB_SUFFIX "" CACHE STRING "Boost library name suffix") + endif(Boost_USE_MULTITHREAD) + MARK_AS_ADVANCED (BOOST_LIB_SUFFIX) + + SET (boostPythonLib boost_python${BOOST_LIB_SUFFIX}) + INCLUDE_DIRECTORIES (${Boost_INCLUDE_DIRS}) + LINK_DIRECTORIES (${Boost_LIBRARY_DIRS}) + else(Boost_FOUND) + MESSAGE (STATUS + "Boost libraries were not found. Cannot build Python bindings for HokuyoAIST.") + endif(Boost_FOUND) +endif(CMAKE_MAJOR_VERSION EQUAL 2 AND CMAKE_MINOR_VERSION GREATER 5) + +if(PYTHON_LIBRARIES AND Boost_FOUND AND HOKUYOAIST_STATIC_LIBS STREQUAL SHARED) + MESSAGE (STATUS "HokuyoAIST Python bindings will be built.") + SET (srcs hokuyoaist.cpp) + + SET (pyModuleTarget hokuyoaist_py) + INCLUDE_DIRECTORIES (${PROJECT_SOURCE_DIR}/src/hokuyoaist ${PYTHON_INCLUDE_PATH}) + add_library(${pyModuleTarget} MODULE ${srcs}) + TARGET_LINK_LIBRARIES (${pyModuleTarget} hokuyoaist ${boostPythonLib} ${PYTHON_LIBRARIES}) + SET_TARGET_PROPERTIES (${pyModuleTarget} PROPERTIES PREFIX "" OUTPUT_NAME "hokuyoaist") + install(TARGETS ${pyModuleTarget} LIBRARY DESTINATION lib/python/site-packages) + + ADD_SUBDIRECTORY (test) +ELSEIF(NOT HOKUYOAIST_STATIC_LIBS STREQUAL SHARED) + MESSAGE (STATUS "HokuyoAIST Python bindings will not be built - must build hokuyoaist as a shared library.") +endif(PYTHON_LIBRARIES AND Boost_FOUND AND HOKUYOAIST_STATIC_LIBS STREQUAL SHARED) + diff --git a/python/hokuyo_aist.cpp b/python/hokuyo_aist.cpp new file mode 100644 index 0000000..21322c3 --- /dev/null +++ b/python/hokuyo_aist.cpp @@ -0,0 +1,221 @@ +#include +using namespace hokuyoaist; + +#include +#include + +#include + +class BaseErrorWrap + : public BaseError, public boost::python::wrapper +{ + public: + BaseErrorWrap(unsigned int desc_code, char const* error_type) + : BaseError(desc_code, error_type) + {} + + BaseErrorWrap(unsigned int desc_code, std::string error_type) + : BaseError(desc_code, error_type.c_str()) + {} + + unsigned int desc_code() const throw() + { + if (boost::python::override f = get_override("desc_code")) + { + return f(); + } + return BaseError::desc_code(); + } + unsigned int default_desc_code() const throw() + { + return BaseError::desc_code(); + } + + char const* error_type() const throw() + { + if (boost::python::override f = get_override("error_type")) + { + return f(); + } + return BaseError::error_type(); + } + char const* default_error_type() const throw() + { + return BaseError::error_type(); + } + + const char* what() throw() + { + if (boost::python::override f = get_override("what")) + { + return f(); + } + return BaseError::what(); + } + const char* default_what() throw() + { + return BaseError::what(); + } +}; + + +class ScanDataWrap : public ScanData, public boost::python::wrapper +{ + public: + ScanDataWrap(void) + : ScanData() + {} + ScanDataWrap(ScanDataWrap const& rhs) + : ScanData(rhs) + {} + + uint32_t range(unsigned int index) + { return ranges_[index]; } + /*unsigned int ranges_length() const + { + if (boost::python::override f = get_override("ranges_length")) + { + return f(); + } + return ScanData::ranges_length(); + }*/ + + uint32_t intensity(unsigned int index) + { return intensities_[index]; } + /*unsigned int intensities_length() const + { + if (boost::python::override f = get_override("intensities_length")) + { + return f(); + } + return ScanData::intensities_length(); + }*/ + + /*bool get_error_status() const + { + if (boost::python::override f = get_override("get_error_status")) + { + return f(); + } + return ScanData::get_error_status(); + } + + std::string error_code_to_string(uint32_t error_code) + { + if (boost::python::override f = get_override("error_code_to_string")) + { + return f(error_code); + } + return ScanData::error_code_to_string(error_code); + } + + unsigned int laser_time_stamp() + { + if (boost::python::override f = get_override("laser_time_stamp")) + { + return f(); + } + return ScanData::laser_time_stamp(); + } + + unsigned int system_time_stamp() + { + if (boost::python::override f = get_override("system_time_stamp")) + { + return f(); + } + return ScanData::system_time_stamp(); + }*/ +}; + +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads1, + get_ranges, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads2, + get_ranges_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads3, + get_ranges_intensities, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads4, + get_ranges_intensities_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads5, + get_new_ranges, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads6, + get_new_ranges_by_angle, 3, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads7, + get_new_ranges_intensities, 1, 4) +BOOST_PYTHON_MEMBER_FUNCTION_OVERLOADS(sensor_overloads8, + get_new_ranges_intensities_by_angle, 3, 4) + +BOOST_PYTHON_MODULE(hokuyoaist) +{ + using namespace boost::python; + + class_("BaseError", init()) + .def("desc_code", &BaseError::desc_code, &BaseErrorWrap::default_desc_code) + .def("error_type", &BaseError::error_type, &BaseErrorWrap::default_error_type) + .def("what", &BaseError::what, &BaseErrorWrap::default_what) + ; + + class_("ScanData") + // TODO: write a wrapper function to copy the data into a python array, because this doesn't work +// .def("ranges", &ScanData::ranges, return_value_policy (), with_custodian_and_ward_postcall<1, 0> ()) +// .def("intensities", &ScanData::intensities, return_value_policy (), with_custodian_and_ward_postcall<1, 0> ()) + .def("range", &ScanDataWrap::range) + .def("intensity", &ScanDataWrap::intensity) + .def("ranges_length", &ScanData::ranges_length) + .def("intensities_length", &ScanData::intensities_length) + .def("get_error_status", &ScanData::get_error_status) + .def("error_code_to_string", &ScanData::error_code_to_string) + .def("laser_time_stamp", &ScanData::laser_time_stamp) + .def("system_time_stamp", &ScanData::system_time_stamp) + .def("model", &ScanData::model) + .def("buffers_provided", &ScanData::buffers_provided) + .def("as_string", &ScanData::as_string) + .def("clean_up", &ScanData::clean_up) + ; + + class_("Sensor") + .def("open", &Sensor::open) + .def("open_with_probing", &Sensor::open_with_probing) + .def("close", &Sensor::close) + .def("is_open", &Sensor::is_open) + .def("set_power", &Sensor::set_power) + .def("set_baud", &Sensor::set_baud) + .def("set_ip", &Sensor::set_ip) + .def("reset", &Sensor::reset) + .def("semi_reset", &Sensor::semi_reset) + .def("set_motor_speed", &Sensor::set_motor_speed) + .def("set_high_sensitivity", &Sensor::set_high_sensitivity) + .def("get_sensor_info", &Sensor::get_sensor_info) + .def("get_time", &Sensor::get_time) + .def("get_raw_time", &Sensor::get_raw_time) + .def("calibrate_time", &Sensor::calibrate_time) + .def("time_offset", &Sensor::time_offset) + .def("set_time_offset", &Sensor::set_time_offset) + .def("drift_rate", &Sensor::drift_rate) + .def("set_drift_rate", &Sensor::set_drift_rate) + .def("skew_alpha", &Sensor::skew_alpha) + .def("set_skew_alpha", &Sensor::set_skew_alpha) + .def("get_ranges", &Sensor::get_ranges, sensor_overloads1()) + .def("get_ranges_by_angle", + &Sensor::get_ranges_by_angle, sensor_overloads2()) + .def("get_ranges_intensities", + &Sensor::get_ranges_intensities, sensor_overloads3()) + .def("get_ranges_intensities_by_angle", + &Sensor::get_ranges_intensities_by_angle, sensor_overloads4()) + .def("get_new_ranges", + &Sensor::get_new_ranges, sensor_overloads5()) + .def("get_new_ranges_by_angle", + &Sensor::get_new_ranges_by_angle, sensor_overloads6()) + .def("get_new_ranges_intensities", + &Sensor::get_new_ranges_intensities, sensor_overloads7()) + .def("get_new_ranges_intensities_by_angle", + &Sensor::get_new_ranges_intensities_by_angle, + sensor_overloads8()) + .def("scip_version", &Sensor::scip_version) + .def("set_verbose", &Sensor::set_verbose) + .def("ignore_unknowns", &Sensor::ignore_unknowns) + .def("set_multiecho_mode", &Sensor::set_multiecho_mode) + .def("step_to_angle", &Sensor::step_to_angle) + .def("angle_to_step", &Sensor::angle_to_step) + ; +} diff --git a/python/test/.svn/all-wcprops b/python/test/.svn/all-wcprops new file mode 100644 index 0000000..38bd539 --- /dev/null +++ b/python/test/.svn/all-wcprops @@ -0,0 +1,17 @@ +K 25 +svn:wc:ra_dav:version-url +V 71 +/svnroot/gearbox/!svn/ver/522/gearbox/trunk/src/hokuyo_aist/python/test +END +hokuyo_aist_example.py +K 25 +svn:wc:ra_dav:version-url +V 94 +/svnroot/gearbox/!svn/ver/522/gearbox/trunk/src/hokuyo_aist/python/test/hokuyo_aist_example.py +END +CMakeLists.txt +K 25 +svn:wc:ra_dav:version-url +V 86 +/svnroot/gearbox/!svn/ver/414/gearbox/trunk/src/hokuyo_aist/python/test/CMakeLists.txt +END diff --git a/python/test/.svn/entries b/python/test/.svn/entries new file mode 100644 index 0000000..f09a569 --- /dev/null +++ b/python/test/.svn/entries @@ -0,0 +1,96 @@ +10 + +dir +531 +https://gearbox.svn.sf.net/svnroot/gearbox/gearbox/trunk/src/hokuyo_aist/python/test +https://gearbox.svn.sf.net/svnroot/gearbox + + + +2010-10-28T09:08:54.881570Z +522 +gbiggs + + + + + + + + + + + + + + +4b7f92db-2845-0410-95f1-b1f669fef64b + +hokuyo_aist_example.py +file + + + + +2010-10-28T09:08:02.000000Z +9d919f60449db78adcb09a4ac3f63442 +2010-10-28T09:08:54.881570Z +522 +gbiggs +has-props + + + + + + + + + + + + + + + + + + + + +3562 + +CMakeLists.txt +file + + + + +2009-07-10T01:17:20.000000Z +070b725b3cf2d386615fffd650a9dd8d +2009-05-29T06:58:49.568476Z +414 +borax00 + + + + + + + + + + + + + + + + + + + + + +57 + diff --git a/python/test/.svn/prop-base/hokuyo_aist_example.py.svn-base b/python/test/.svn/prop-base/hokuyo_aist_example.py.svn-base new file mode 100644 index 0000000..869ac71 --- /dev/null +++ b/python/test/.svn/prop-base/hokuyo_aist_example.py.svn-base @@ -0,0 +1,5 @@ +K 14 +svn:executable +V 1 +* +END diff --git a/python/test/.svn/text-base/CMakeLists.txt.svn-base b/python/test/.svn/text-base/CMakeLists.txt.svn-base new file mode 100644 index 0000000..764e631 --- /dev/null +++ b/python/test/.svn/text-base/CMakeLists.txt.svn-base @@ -0,0 +1 @@ +GBX_ADD_SHARED_FILES(hokuyo_aist hokuyo_aist_example.py) diff --git a/python/test/.svn/text-base/hokuyo_aist_example.py.svn-base b/python/test/.svn/text-base/hokuyo_aist_example.py.svn-base new file mode 100644 index 0000000..c38e488 --- /dev/null +++ b/python/test/.svn/text-base/hokuyo_aist_example.py.svn-base @@ -0,0 +1,91 @@ +#!/usr/bin/env python + +import hokuyo_aist +from optparse import OptionParser + +def main(): + parser = OptionParser() + parser.add_option('-c', '--clustercount', dest='cluster_count', + type='int', default='1', + help='Cluster count [default: %default]') + parser.add_option('-e', '--endangle', dest='end_angle', type='float', + default='0', + help='End angle to get ranges to [default: %default]') + parser.add_option('-f', '--firststep', dest='first_step', type='int', + default='-1', + help='First step to get ranges from [default: %default]') + parser.add_option('-l', '--laststep', dest='last_step', type='int', + default='-1', + help='Last step to get ranges to [default: %default]') + parser.add_option('-n', '--new', dest='get_new', action='store_true', + default='False', help='Get new ranges instead of latest \ +ranges [default: %default]') + parser.add_option('-o', '--portoptions', dest='port_options', type='string', + default='type=serial,device=/dev/ttyACM0,timeout=1', + help='Port options (see flexiport library) [default: %default]') + parser.add_option('-s', '--startangle', dest='start_angle', type='float', + default='0', + help='Start angle to get ranges from [default: %default]') + parser.add_option('-v', '--verbose', dest='verbose', action='store_true', + default='False', + help='Put the hokuyo_aist library into verbose mode \ +[Default: %default]') + + # Scan command line arguments + options, args = parser.parse_args() + + try: + # Create an instance of a laser scanner object + laser = hokuyo_aist.Sensor() + if options.verbose == True: + # Set verbose mode so we see more information in stderr + laser.set_verbose(True) + + # Open the laser + laser.open(options.port_options) + # Turn the laser on + laser.set_power(True) + + # Get some laser info + #print 'Laser sensor information:' + #info = hokuyo_aist.SensorInfo info() + #laser.get_sensor_info(info) + #print info.as_string() + + # Get range data + data = hokuyo_aist.ScanData() + if (options.first_step == -1 and options.last_step == -1) and \ + (options.start_angle == 0 and options.end_angle == 0): + # Get all ranges + if options.get_new == True: + laser.get_new_ranges(data, -1, -1, options.cluster_count) + else: + laser.get_ranges(data, -1, -1, options.cluster_count) + elif options.first_step != -1 or options.last_step != -1: + # Get by step + if options.get_new == True: + laser.get_new_ranges(data, options.first_step, options.last_step, options.cluster_count) + else: + laser.get_ranges(data, options.first_step, options.last_step, options.cluster_count) + else: + # Get by angle + if options.get_new == True: + laser.get_new_ranges_by_angle(data, options.start_angle, options.end_angle, options.cluster_count) + else: + laser.get_ranges_by_angle(data, options.start_angle, options.end_angle, options.cluster_count) + + print 'Laser range data:' + print data.as_string() + + # Close the laser + laser.close() + + except hokuyo_aist.BaseError, e: + print 'Caught exception: ' + e.what() + return 1 + return 0 + + +if __name__ == '__main__': + main() + diff --git a/python/test/CMakeLists.txt b/python/test/CMakeLists.txt new file mode 100644 index 0000000..e69de29 diff --git a/python/test/hokuyo_aist_example.py b/python/test/hokuyo_aist_example.py new file mode 100755 index 0000000..1eeab14 --- /dev/null +++ b/python/test/hokuyo_aist_example.py @@ -0,0 +1,91 @@ +#!/usr/bin/env python + +import hokuyoaist +from optparse import OptionParser + +def main(): + parser = OptionParser() + parser.add_option('-c', '--clustercount', dest='cluster_count', + type='int', default='1', + help='Cluster count [default: %default]') + parser.add_option('-e', '--endangle', dest='end_angle', type='float', + default='0', + help='End angle to get ranges to [default: %default]') + parser.add_option('-f', '--firststep', dest='first_step', type='int', + default='-1', + help='First step to get ranges from [default: %default]') + parser.add_option('-l', '--laststep', dest='last_step', type='int', + default='-1', + help='Last step to get ranges to [default: %default]') + parser.add_option('-n', '--new', dest='get_new', action='store_true', + default='False', help='Get new ranges instead of latest \ +ranges [default: %default]') + parser.add_option('-o', '--portoptions', dest='port_options', type='string', + default='type=serial,device=/dev/ttyACM0,timeout=1', + help='Port options (see flexiport library) [default: %default]') + parser.add_option('-s', '--startangle', dest='start_angle', type='float', + default='0', + help='Start angle to get ranges from [default: %default]') + parser.add_option('-v', '--verbose', dest='verbose', action='store_true', + default='False', + help='Put the hokuyoaist library into verbose mode \ +[Default: %default]') + + # Scan command line arguments + options, args = parser.parse_args() + + try: + # Create an instance of a laser scanner object + laser = hokuyoaist.Sensor() + if options.verbose == True: + # Set verbose mode so we see more information in stderr + laser.set_verbose(True) + + # Open the laser + laser.open(options.port_options) + # Turn the laser on + laser.set_power(True) + + # Get some laser info + #print 'Laser sensor information:' + #info = hokuyoaist.SensorInfo info() + #laser.get_sensor_info(info) + #print info.as_string() + + # Get range data + data = hokuyoaist.ScanData() + if (options.first_step == -1 and options.last_step == -1) and \ + (options.start_angle == 0 and options.end_angle == 0): + # Get all ranges + if options.get_new == True: + laser.get_new_ranges(data, -1, -1, options.cluster_count) + else: + laser.get_ranges(data, -1, -1, options.cluster_count) + elif options.first_step != -1 or options.last_step != -1: + # Get by step + if options.get_new == True: + laser.get_new_ranges(data, options.first_step, options.last_step, options.cluster_count) + else: + laser.get_ranges(data, options.first_step, options.last_step, options.cluster_count) + else: + # Get by angle + if options.get_new == True: + laser.get_new_ranges_by_angle(data, options.start_angle, options.end_angle, options.cluster_count) + else: + laser.get_ranges_by_angle(data, options.start_angle, options.end_angle, options.cluster_count) + + print 'Laser range data:' + print data.as_string() + + # Close the laser + laser.close() + + except hokuyoaist.BaseError, e: + print 'Caught exception: ' + e.what() + return 1 + return 0 + + +if __name__ == '__main__': + main() + diff --git a/src/CMakeLists.txt b/src/CMakeLists.txt new file mode 100644 index 0000000..f1b8c2a --- /dev/null +++ b/src/CMakeLists.txt @@ -0,0 +1,26 @@ +set(srcs hokuyo_errors.cpp + scan_data.cpp + sensor.cpp + sensor_info.cpp + ) + +include_directories(${PROJECT_SOURCE_DIR}/include) +include_directories(${PROJECT_BINARY_DIR}/include) +include_directories(${Flexiport_INCLUDE_DIRS}) +add_library(${PROJECT_NAME_LOWER} ${LIB_TYPE} ${srcs}) +target_link_libraries(${PROJECT_NAME_LOWER} ${Flexiport_LIBRARIES}) +if(HAVE_CLOCK_GETTIME) + target_link_libraries(${PROJECT_NAME_LOWER} rt) +endif(HAVE_CLOCK_GETTIME) +set_target_properties(${PROJECT_NAME_LOWER} PROPERTIES + VERSION ${PROJECT_VERSION} + SOVERSION ${PROJECT_VERSION_MAJOR}) +install(TARGETS ${PROJECT_NAME_LOWER} + EXPORT ${PROJECT_NAME_LOWER} + RUNTIME DESTINATION ${BIN_INSTALL_DIR} COMPONENT library + LIBRARY DESTINATION ${LIB_INSTALL_DIR} COMPONENT library + ARCHIVE DESTINATION ${LIB_INSTALL_DIR} COMPONENT library) +install(EXPORT ${PROJECT_NAME_LOWER} + DESTINATION ${LIB_INSTALL_DIR}/${PROJECT_NAME_LOWER} + FILE ${PROJECT_NAME_LOWER}Depends.cmake) + diff --git a/src/hokuyo_errors.cpp b/src/hokuyo_errors.cpp new file mode 100644 index 0000000..ca9dcdb --- /dev/null +++ b/src/hokuyo_errors.cpp @@ -0,0 +1,474 @@ +/* HokuyoAIST + * + * Implementation of the exceptions. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include + +#include +#include + +namespace hokuyoaist +{ + +// error must be null-terminated +std::string scip2_error_to_string(char const* const error, + char const* const cmd) +{ + std::stringstream ss; + + // Check for non-errors + if(error[0] == '0' && error[1] == '0') + return "Status OK - 00"; + else if(error[0] == '9' && error[1] == '9') + return "Status OK - 99"; + + // Check for universal errors + if(error[0] == '0' && error[1] == 'A') + return "Unable to create transmission data or reply command internally"; + else if(error[0] == '0' && error[1] == 'B') + return "Buffer shortage or command repeated that is already processed"; + else if(error[0] == '0' && error[1] == 'C') + return "Command with insufficient parameters 1"; + else if(error[0] == '0' && error[1] == 'D') + return "Undefined command 1"; + else if(error[0] == '0' && error[1] == 'E') + return "Undefined command 2"; + else if(error[0] == '0' && error[1] == 'F') + return "Command with insufficient parameters 2"; + else if(error[0] == '0' && error[1] == 'G') + return "String character in command exceeds 16 letters"; + else if(error[0] == '0' && error[1] == 'H') + return "String character has invalid letters"; + else if(error[0] == '0' && error[1] == 'I') + return "Sensor is now in firmware update mode"; + + int error_code = atoi(error); + + if(cmd[0] == 'B' && cmd[1] == 'M') + { + switch(error_code) + { + case 1: + return "Unable to control due to laser malfunction"; + case 2: + return "Laser is already on"; + } + } +// No info in the manual for this. +// else if(cmd[0] == 'Q' && cmd[1] == 'T') +// { +// switch(error_code) +// { +// default: +// std::stringstream ss; +// ss << "Unknown error code " << error_code << +// " for command " << cmd[0] << cmd[1]; +// return ss.str (); +// } +// } + else if(((cmd[0] == 'G' || cmd[0] == 'H') && + (cmd[1] == 'D' || cmd[1] == 'E')) || + (cmd[0] == 'G' && cmd[1] == 'S')) + { + switch(error_code) + { + case 1: + return "Starting step has non-numeric value"; + case 2: + return "Ending step has non-numeric value"; + case 3: + return "Cluster count has non-numeric value"; + case 4: + return "Ending step is out of range"; + case 5: + return "Ending step is smaller than start step"; + case 10: + return "Laser is off"; + default: + if(error_code >= 50) + ss << "Hardware error: " << error_code; + else + ss << "Unknown error code " << error_code << + " for command " << cmd[0] << cmd[1]; + + return ss.str(); + } + } + else if(((cmd[0] == 'M' || cmd[0] == 'N') && + (cmd[1] == 'D' || cmd[1] == 'E')) || + (cmd[0] == 'M' && cmd[1] == 'S')) + { + switch(error_code) + { + case 1: + return "Starting step has non-numeric value"; + case 2: + return "Ending step has non-numeric value"; + case 3: + return "Cluster count has non-numeric value"; + case 4: + return "Ending step is out of range"; + case 5: + return "Ending step is smaller than start step"; + case 6: + return "Scan interval has non-numeric value"; + case 7: + return "Number of scans is non-numeric"; + default: + if(error_code >= 21 && error_code <= 49) + { + return "Processing stopped to verify error. " + "This function is not yet supported by hokuyoaist."; + } + else if(error_code >= 50 && error_code <= 97) + ss << "Hardware error: " << error_code; + else if(error_code == 98) + { + return "Resumption of processing after confirming normal " + "laser opteration. This function is not yet supported " + "by hokuyoaist."; + } + else + ss << "Unknown error code " << error_code << + " for command " << cmd[0] << cmd[1]; + + return ss.str(); + } + } + else if(cmd[0] == 'T' && cmd[1] == 'M') + { + switch(error_code) + { + case 1: + return "Invalid control code"; + case 2: + return "Adjust mode on command received when sensor's adjust " + "mode is already on"; + case 3: + return "Adjust mode off command received when sensor's adjust " + "mode is already off"; + case 4: + return "Adjust mode is off when requested time"; + } + } + else if(cmd[0] == 'S' && cmd[1] == 'S') + { + switch(error_code) + { + case 1: + return "Baud rate has non-numeric value"; + case 2: + return "Invalid baud rate"; + case 3: + return "Sensor is already running at that baud rate"; + case 4: + return "Not compatible with the sensor model"; + } + } + else if(cmd[0] == 'C' && cmd[1] == 'R') + { + switch(error_code) + { + case 1: + return "Invalid speed"; + case 2: + return "Speed is out of range"; + case 3: + return "Motor is already running at that speed"; + case 4: + return "Not compatible with the sensor model"; + } + } + else if(cmd[0] == 'H' && cmd[1] == 'S') + { + switch(error_code) + { + case 1: + return "Parameter error"; + case 2: + return "Already running in the set mode"; + case 3: + return "Not compatible with the sensor model"; + } + } +// No info in the manual for this. +// else if(cmd[0] == 'R' && cmd[1] == 'S') +// { +// switch(error_code) +// { +// case : +// return ""; +// default: +// std::stringstream ss; +// ss << "Unknown error code " << error_code << +// " for command " << cmd[0] << cmd[1]; +// return ss.str (); +// } +// } +// No info in the manual for this. +// else if(cmd[0] == 'V' && cmd[1] == 'V') +// { +// switch(error_code) +// { +// case : +// return ""; +// } +// } +// No info in the manual for this. +// else if(cmd[0] == 'P' && cmd[1] == 'P') +// { +// switch(error_code) +// { +// case : +// return ""; +// } +// } +// No info in the manual for this. +// else if(cmd[0] == 'I' && cmd[1] == 'I') +// { +// switch(error_code) +// { +// case : +// return ""; +// } +// } + else if(cmd[0] == 'D' && cmd[1] == 'B') + { + switch(error_code) + { + case 1: + return "Parameter error"; + case 2: + return "Already running in the set mode"; + case 3: + return "Already returned to normal mode"; + case 4: + return "Selected mode does not match the SCIP version in use"; + case 5: + return "Sensor has a physical malfunction"; + } + } + else + { + ss << "Unknown command: " << cmd[0] << cmd[1]; + return ss.str(); + } + + // Known commands with unknown error codes fall through to here + ss << "Unknown error code " << error_code << " for command " << cmd[0] << + cmd[1]; + return ss.str(); +} + + +std::string desc_code_to_string(unsigned int code) +{ + static char const* const descriptions[] = { +/* 0 */ "Timed out trying to read a line", +/* 1 */ "No data received when trying to read a line.", +/* 2 */ "Invalid data index.", +/* 3 */ "Port is not open.", +/* 4 */ "Unknown SCIP version.", +/* 5 */ "Cannot change baud rate of non-serial connection.", +/* 6 */ "Bad baud rate: ", +/* 7 */ "SCIP version 1 does not support the reset command.", +/* 8 */ "SCIP version 1 does not support the set motor speed command.", +/* 9 */ "Invalid motor speed.", +/* 10 */ "SCIP version 1 does not support the high sensitivity command.", +/* 11 */ "No info object provided.", +/* 12 */ "SCIP version 1 does not support the get time command.", +/* 13 */ "No data received. Check data error code.", +/* 14 */ "Start step is out of range.", +/* 15 */ "End step is out of range.", +/* 16 */ "SCIP version 1 does not support the get new ranges command.", +/* 17 */ "SCIP version 1 does not support the get new ranges and intensities command.", +/* 18 */ "Timed out while skipping.", +/* 19 */ "Failed to write command byte.", +/* 20 */ "Failed to write command parameters.", +/* 21 */ "Failed to write termination character.", +/* 22 */ "SCIP versions 1 and 2 failed.", +/* 23 */ "Out-of-range firmware version.", +/* 24 */ "Invalid checksum: ", +/* 25 */ "Read a different number of range or intensity readings than were asked for.", +/* 26 */ "Found line feed in a data block.", +/* 27 */ "Unknown line: ", +/* 28 */ "Parse error: ", +/* 29 */ "'FIRM:' was not found when checking firmware version.", +/* 30 */ "Bad response.", +/* 31 */ "Incorrect command echo.", +/* 32 */ "Incorrect parameters echo for command.", +/* 33 */ "Not enough bytes to calculate checksum.", +/* 34 */ "Incorrect line length received.", +/* 35 */ "SCIP version 1 does not support the semi-reset command.", +/* 36 */ "SCIP version 1 does not support the get ranges and intensities command.", +/* 37 */ "Error configuring IP address.", +/* 38 */ "Did not receive a full line." + }; + + return std::string(descriptions[code]); +} + + +BaseError::BaseError(unsigned int desc_code, char const* error_type) + : desc_code_(desc_code) +{ + strncpy(error_type_, error_type, 32); +} + + +BaseError::BaseError(BaseError const& rhs) + : desc_code_(rhs.desc_code()) +{ + strncpy(error_type_, rhs.error_type(), 32); +} + + +const char* BaseError::what() throw() +{ + ss << error_type_ << " (" << desc_code_ << "): " << + desc_code_to_string(desc_code_); + return ss.str().c_str(); +} + + +const char* BaudrateError::what() throw() +{ + RuntimeError::what(); + ss << baud_; + return ss.str().c_str(); +} + + +const char* ChecksumError::what() throw() +{ + ProtocolError::what(); + ss << "expected " << expected_ << ", calculated " << calculated_; + return ss.str().c_str(); +} + + +UnknownLineError::UnknownLineError(char const* const line) + : ProtocolError(27, "UnknownLineError") +{ + strncpy(line_, line, 128); +} + + +UnknownLineError::UnknownLineError(UnknownLineError const& rhs) + : ProtocolError(rhs) +{ + strncpy(line_, rhs.line(), 128); +} + + +const char* UnknownLineError::what() throw() +{ + ProtocolError::what(); + ss << line_; + return ss.str().c_str(); +} + + +ParseError::ParseError(char const* const line, char const* const type) + : ProtocolError(28, "ParseError") +{ + strncpy(line_, line, 128); + strncpy(type_, type, 16); +} + + +ParseError::ParseError(ParseError const& rhs) + : ProtocolError(rhs) +{ + strncpy(line_, rhs.line(), 128); + strncpy(type_, rhs.type(), 16); +} + + +const char* ParseError::what() throw() +{ + ProtocolError::what(); + ss << "Line type: " << type_ << ". Line: " << line_; + return ss.str().c_str(); +} + + +const char* ResponseError::what() throw() +{ + ProtocolError::what(); + ss << " Command: " << cmd_[0] << cmd_[1]; + ss << " Error : (" << error_[0] << error_[1] << ") " << + scip2_error_to_string(error_, cmd_); + return ss.str().c_str(); +} + + +const char* Scip1ResponseError::what() throw() +{ + ProtocolError::what(); + ss << " Command: " << cmd_; + ss << " Error : " << error_; + return ss.str().c_str(); +} + + +const char* CommandEchoError::what() throw() +{ + ProtocolError::what(); + ss << " Command: " << cmd_[0] << cmd_[1]; + ss << " Received echo: " << echo_[0] << echo_[1]; + return ss.str().c_str(); +} + + +const char* ParamEchoError::what() throw() +{ + ProtocolError::what(); + ss << " Command: " << cmd_[0] << cmd_[1]; + return ss.str().c_str(); +} + + +const char* InsufficientBytesError::what() throw() +{ + ProtocolError::what(); + ss << " Number of bytes: " << num_; + ss << " Line length: " << line_length_; + return ss.str().c_str(); +} + + +const char* LineLengthError::what() throw() +{ + ProtocolError::what(); + ss << " Received length: " << length_; + ss << " Expected line length: " << expected_; + return ss.str().c_str(); +} + +}; // namespace hokuyoaist + diff --git a/src/scan_data.cpp b/src/scan_data.cpp new file mode 100644 index 0000000..a11c983 --- /dev/null +++ b/src/scan_data.cpp @@ -0,0 +1,455 @@ +/* HokuyoAIST + * + * Implementation of the scan data object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include + +#include + +#include +#include + +using namespace hokuyoaist; + +/////////////////////////////////////////////////////////////////////////////// +// ScanData class +/////////////////////////////////////////////////////////////////////////////// + +ScanData::ScanData() + : ranges_(0), intensities_(0), ranges_length_(0), + intensities_length_(0), error_(false), laser_time_(0), system_time_(0), + model_(MODEL_UNKNOWN), buffers_provided_(false) +{ +} + + +ScanData::ScanData(uint32_t* const ranges_buffer, + unsigned int ranges_length, uint32_t* const intensities_buffer, + unsigned int intensities_length) + : ranges_(ranges_buffer), intensities_(intensities_buffer), + ranges_length_(ranges_length), intensities_length_(intensities_length), + error_(false), laser_time_(0), system_time_(0), model_(MODEL_UNKNOWN), + buffers_provided_(true) +{ +} + + +ScanData::ScanData(ScanData const& rhs) +{ + ranges_length_ = rhs.ranges_length(); + intensities_length_ = rhs.intensities_length(); + if(ranges_length_ == 0) + ranges_ = 0; + else + { + try + { + ranges_ = new uint32_t[ranges_length_]; + } + catch(std::bad_alloc& e) + { + ranges_length_ = 0; + throw; + } + memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * ranges_length_); + } + if(intensities_length_ == 0) + intensities_ = 0; + else + { + try + { + intensities_ = new uint32_t[intensities_length_]; + } + catch(std::bad_alloc& e) + { + intensities_length_ = 0; + throw; + } + memcpy(intensities_, rhs.ranges(), + sizeof(uint32_t) * intensities_length_); + } + error_ = rhs.get_error_status(); + laser_time_ = rhs.laser_time_stamp(); + system_time_ = rhs.system_time_stamp(); + model_ = rhs.model(); + buffers_provided_ = rhs.buffers_provided(); +} + + +ScanData::~ScanData() +{ + if(!buffers_provided_) + { + if (ranges_ != 0) + { + delete[] ranges_; + ranges_ = 0; + } + if (intensities_ != 0) + { + delete[] intensities_; + intensities_ = 0; + } + } +} + + +std::string ScanData::error_code_to_string(uint32_t error_code) +{ + if(model_ == MODEL_UTM30LX) + { + switch(error_code) + { + case 1: + return "No object in the range."; + case 2: + return "Object is too near (internal error)."; + case 3: + return "Measurement error (may be due to interference)."; + case 4: + return "Object out of range (at the near end)."; + case 5: + return "Other error."; + default: + std::stringstream ss; + ss << "Unknown error code: " << error_code; + return ss.str(); + } + } + else + { + switch(error_code) + { + case 0: + return "Detected object is possibly at 22m."; + case 1: + return "Reflected light has low intensity."; + case 2: + return "Reflected light has low intensity."; + case 3: + return "Reflected light has low intensity."; + case 4: + return "Reflected light has low intensity."; + case 5: + return "Reflected light has low intensity."; + case 6: + return "Possibility of detected object is at 5.7m."; + case 7: + return "Distance data on the preceding and succeeding steps " + "have errors."; + case 8: + return "Others."; + case 9: + return "The same step had error in the last two scan."; + case 10: + return "Others."; + case 11: + return "Others."; + case 12: + return "Others."; + case 13: + return "Others."; + case 14: + return "Others."; + case 15: + return "Others."; + case 16: + return "Possibility of detected object is in the range " + "4096mm."; + case 17: + return "Others."; + case 18: + return "Unspecified."; + case 19: + return "Non-measurable distance."; + default: + std::stringstream ss; + ss << "Unknown error code: " << error_code; + return ss.str(); + } + } +} + + +ScanData& ScanData::operator=(ScanData const& rhs) +{ + unsigned int rhslength = rhs.ranges_length(); + if(rhslength == 0) + { + ranges_ = 0; + ranges_length_ = 0; + } + else + { + if(rhslength != ranges_length_) + { + if(!buffers_provided_ && ranges_ != 0) + { + // Just copy + memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * rhslength); + } + else + { + // Copy the data into a temporary variable pointing to new space + // (prevents dangling pointers on allocation error and prevents + // self-assignment making a mess). + uint32_t* new_data = new uint32_t[rhslength]; + memcpy(new_data, rhs.ranges(), sizeof(uint32_t) * rhslength); + if(ranges_ != 0) + delete[] ranges_; + ranges_ = new_data; + ranges_length_ = rhs.ranges_length(); + } + } + else + { + // If lengths are the same, no need to reallocate + memcpy(ranges_, rhs.ranges(), sizeof(uint32_t) * rhslength); + } + } + + rhslength = rhs.intensities_length(); + if(rhslength == 0) + { + intensities_ = 0; + intensities_length_ = 0; + } + else + { + if(rhslength != intensities_length_) + { + if(!buffers_provided_ && intensities_ != 0) + { + // Just copy + memcpy(intensities_, rhs.intensities(), + sizeof(uint32_t) * rhslength); + } + else + { + // Copy the data into a temporary variable pointing to new + // space (prevents dangling pointers on allocation error and + // prevents self-assignment making a mess). + uint32_t* new_data = new uint32_t[rhslength]; + memcpy(new_data, rhs.intensities(), + sizeof(uint32_t) * rhslength); + if(intensities_ != 0) + delete[] intensities_; + intensities_ = new_data; + intensities_length_ = rhs.intensities_length(); + } + } + else + { + // If lengths are the same, no need to reallocate + memcpy(intensities_, rhs.intensities(), + sizeof(uint32_t) * rhslength); + } + } + + error_ = rhs.get_error_status(); + laser_time_ = rhs.laser_time_stamp(); + system_time_ = rhs.system_time_stamp(); + model_ = rhs.model(); + buffers_provided_ = rhs.buffers_provided(); + + return *this; +} + + +uint32_t ScanData::operator[](unsigned int index) +{ + if(index >= ranges_length_) + throw IndexError(); + return ranges_[index]; +} + + +std::string ScanData::as_string() +{ + std::stringstream ss; + + if(ranges_ != 0) + { + ss << ranges_length_ << " ranges from model "; + ss << model_to_string(model_) << ":\n"; + for(unsigned int ii(0); ii < ranges_length_; ii++) + ss << ranges_[ii] << '\t'; + ss << '\n'; + } + if(intensities_ != 0) + { + ss << intensities_length_ << " intensities from model "; + ss << model_to_string(model_) << ":\n"; + for(unsigned int ii(0); ii < intensities_length_; ii++) + ss << intensities_[ii] << '\t'; + ss << '\n'; + } + + if(error_) + { + ss << "Detected data errors:\n"; + for(unsigned int ii = 0; ii < ranges_length_; ii++) + { + if(ranges_[ii] < 20) + ss << ii << ": " << error_code_to_string(ranges_[ii]) << '\n'; + } + } + else + ss << "No data errors.\n"; + ss << "Laser time stamp: " << laser_time_ << "ms\n"; + ss << "System time stamp: " << system_time_ << "ns\n"; + + return ss.str(); +} + + +void ScanData::clean_up() +{ + if(!buffers_provided_) + { + if(ranges_ != 0) + delete[] ranges_; + ranges_ = 0; + if(intensities_ != 0) + delete[] intensities_; + intensities_ = 0; + } + ranges_length_ = 0; + intensities_length_ = 0; + error_ = false; + laser_time_ = 0; + system_time_ = 0; +} + + +void ScanData::allocate_data(unsigned int length, bool include_intensities) +{ + // If buffers have been provided, automatic allocation is off. + if(buffers_provided_) + return; + + // If no data yet, allocate new + if(ranges_ == 0) + { + try + { + ranges_ = new uint32_t[length]; + } + catch(std::bad_alloc& e) + { + ranges_length_ = 0; + throw; + } + ranges_length_ = length; + } + // If there is data, reallocate only if the length is different + else if(length != ranges_length_) + { + delete[] ranges_; + try + { + ranges_ = new uint32_t[length]; + } + catch(std::bad_alloc& e) + { + ranges_length_ = 0; + throw; + } + ranges_length_ = length; + } + // Else data is already allocated to the right length, so do nothing + + if(include_intensities) + { + // If no data yet, allocate new + if(intensities_ == 0) + { + try + { + intensities_ = new uint32_t[length]; + } + catch(std::bad_alloc& e) + { + intensities_length_ = 0; + throw; + } + intensities_length_ = length; + } + // If there is data, reallocate only if the length is different + else if(length != intensities_length_) + { + delete[] intensities_; + try + { + intensities_ = new uint32_t[length]; + } + catch(std::bad_alloc& e) + { + intensities_length_ = 0; + throw; + } + intensities_length_ = length; + } + // Else data is already allocated to the right length, so do nothing + } + else if(intensities_ != 0) + { + // If not told to allocate space for intensity data and it exists, + // remove it + delete[] intensities_; + intensities_ = 0; + intensities_length_ = 0; + } +} + + +void ScanData::write_range(unsigned int index, uint32_t value) +{ + if(ranges_ != 0) + { + if(index >= ranges_length_) + throw IndexError(); + ranges_[index] = value; + if(ranges_[index] < 20) + error_ = true; + } +} + + +void ScanData::write_intensity(unsigned int index, uint32_t value) +{ + if(intensities_ != 0) + { + if(index >= intensities_length_) + throw IndexError(); + intensities_[index] = value; + if(intensities_[index] < 20) + error_ = true; + } +} + diff --git a/src/sensor.cpp b/src/sensor.cpp new file mode 100644 index 0000000..9b35bc0 --- /dev/null +++ b/src/sensor.cpp @@ -0,0 +1,2630 @@ +/* HokuyoAIST + * + * Implementation of the sensor object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include +#include +#include +#include +using namespace hokuyoaist; + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#if defined(WIN32) + #include // For Sleep() + #define __func__ __FUNCTION__ +#else + #include +#endif + +namespace hokuyoaist +{ + +// SCIP1: 66 bytes (64 bytes of data + line feed + 0) +unsigned int const SCIP1_LINE_LENGTH = 66; +// SCIP2: 67 bytes (64 bytes of data + checksum byte + line feed + 0) +unsigned int const SCIP2_LINE_LENGTH = 67; +// Size of a full data block in range data +unsigned int const DATA_BLOCK_LENGTH = 64; + +/////////////////////////////////////////////////////////////////////////////// +// SCIP protocol version 1 notes +/////////////////////////////////////////////////////////////////////////////// + +/* | = byte boundary, ... indicates variable byte block (max 64 bytes), +(x) = x byte block + - No checksum + - Host to sensor: Command | Parameters... | LF + - Sensor to host: Command | Parameters... | LF | Status | LF | Data... | LF | LF + - Where a block of data would take more than 64 bytes, a line feed is inserted + every 64 bytes. + - Status 0 is OK, anything else is an error. + +L Power + L|Control code|LF + L|Control code|LF|Status|LF|LF + 3 byte command block +G Get data + G|Start(3)|End(3)|Cluster(2)|LF + G|Start(3)|End(3)|Cluster(2)|LF|Status|LF|Data...|LF|LF + 10 byte command block +S Set baud rate + S|Baud rate(6)|Reserved(7)|LF + S|Baud rate(6)|Reserved(7)|Status|LF|LF| + 16 byte command block +V Version info + V|LF + V|LF|Status...|LF|Vendor...|LF|Product...|LF|Firmware...|LF|Protocol...|LF| + Serial...|LF|LF + 2 byte command block +*/ + +// SCIP protocol version 2 notes +/////////////////////////////////////////////////////////////////////////////// + +/* | = byte boundary, ... indicates variable byte block (max 64 bytes), +(x) = x byte block + - We don't use the string block (which can be up to 16 bytes) so it's marked + as size 0 and ignored in the command definitions below. + - Host to sensor: Command(2) | Parameters... | String(0) | LF + - Sensor to host: Command(2) | Parameters... | String(0) | LF | Status(2) | + Sum | LF + - Each data row: Data (max 64) | Sum | LF + - Data rows are broken after a maximum of 64 bytes, each one having a checksum + and a line feed. + - Status codes 00 and 99 are OK, anything else is an error. + - Checksum is calculated by... well, see the code. + - BIG NOTE: The UTM-30LX has a probable bug in its response to the II code + whereby it does not include anything after and including "<-" in the + checksum calculation. + +VV Version info + V|V|LF + V|V|LF|Status(2)|Sum|LF|Vendor...|;|Sum|LF|Product...|;|Sum|LF| + Firmware...|;|Sum|LF|Protocol...|;|Sum|LF|Serial...|;|Sum|LF|LF + 3 byte command block +PP Specification info + P|P|LF + P|P|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|MinRange...|;|Sum|LF| + MaxRange...|;|Sum|LF|TotalSteps...|;|Sum|LF|FirstStep...|;|Sum|LF| + LastStep...|;|Sum|LF|FrontStep...|;|Sum|LF|MotorSpeed...|;|Sum|LF|LF + 3 byte command block +II Status info + I|I|LF + I|I|LF|Status(2)|Sum|LF|Model...|;|Sum|LF|Power...|;|Sum|LF| + MotorSpeed...|;|Sum|LF|Mode...|;|Sum|LF|Baud...|;|Sum|LF|Time...|;|Sum| + LF|Diagnostic...|;|Sum|LF|LF + 3 byte command block +BM Power on + B|M|LF + B|M|LF|Status(2)|Sum|LF|LF + 3 byte command block +QT Power off + Q|T|LF + Q|T|LF|Status(2)|Sum|LF|LF + 3 byte command block +SS Set baud rate + S|S|Baud(6)|LF + S|S|Baud(6)|LF|Status(2)|Sum|LF|LF + 9 byte command block +MDMS Get new data + M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF + M|D/S|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF| + Status(2)|Sum|LF|Data...|LF|LF + 16 byte command block +ME Get new data, including intensity data + M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF + M|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF| + Status(2)|Sum|LF|Data...|LF|LF + 16 byte command block +GDGS Get latest data + G|D/S|Start(4)|End(4)|Cluster(2)|LF + G|D/S|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF + 12 byte command block +GE Get latest data, including intensity data + G|E|Start(4)|End(4)|Cluster(2)|LF + G|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF + 12 byte command block +CR Set motor speed + C|R|Speed(2)|LF + C|R|Speed(2)|LF|Status(2)|Sum|LF|LF + 5 byte command block +TM Get sensor time + T|M|Code|LF + T|M|Code|LF|Status(2)|Sum|LF[|Time(4)|Sum|LF|LF + 4 byte command block + Optional part only comes back for control code 1. +RS Reset + R|S|LF + R|S|LF|Status(2)|Sum|LF|LF + 3 byte command block +RT Same as reset, but does not stop the motor or alter serial speed + R|T|LF + R|T|LF|Status(2)|Sum|LF|LF +ND Multi-echo version of MD + N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF + N|D|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF| + Status(2)|Sum|LF|Data...|LF|LF + 16 byte command block + Returned scan data may include echo data separated by '&'. +NE Multi-echo version of ME + N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number(2)|LF + N|E|Start(4)|End(4)|Cluster(2)|Interval(1)|Number remaining(2)|LF| + Status(2)|Sum|LF|Data...|LF|LF + 16 byte command block + Returned scan data may include echo data separated by '&'. +HD Multi-echo version of GD + H|D|Start(4)|End(4)|Cluster(2)|LF + H|D|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF + 12 byte command block + Returned scan data may include echo data separated by '&'. +HE Multi-echo version of GE + H|E|Start(4)|End(4)|Cluster(2)|LF + H|E|Start(4)|End(4)|Cluster(2)|LF|Status(2)|Sum|LF|Data...|LF|LF + 12 byte command block + Returned scan data may include echo data separated by '&'. +HS Set high sensitivity mode + H|S|On/Off(1)|LF + H|S|On/Off(1)|LF|Status(2)|Sum|LF|LF + 3 byte command block + Use '0' for on, '1' for off. +$IP Change IP address settings + $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF + $|I|P|IP(15)| |Subnet mask(15)| |Default gateway(15)|LF|Status(2)| + Sum|LF|LF + Only available on models supporting ethernet connections. + Does not conform to standard command format. + Laser will require a restart after use (wait for response first). +DB Simulate sensor failure modes + D|B|Command(2)|LF + D|B|Command(2)|LF|Status(2)|Sum|LF|LF + Where command is one of: + "02" Enter failure state. + "03" While sending data, go through normal -> malfunction -> normal. + "04" While sending data, go through normal -> malfunction -> failure. + "05" While sending data, go through normal -> failure. + "10" Return to normal operation. + 4 byte command block +*/ + +/////////////////////////////////////////////////////////////////////////////// +// Utility functions +/////////////////////////////////////////////////////////////////////////////// + +unsigned int decode_2_byte_value(char* data) +{ + unsigned int byte1, byte2; + + byte1 = data[0] - 0x30; + byte2 = data[1] - 0x30; + + return (byte1 << 6) + (byte2); +} + + +unsigned int decode_3_byte_value(char* data) +{ + unsigned int byte1, byte2, byte3; + + byte1 = data[0] - 0x30; + byte2 = data[1] - 0x30; + byte3 = data[2] - 0x30; + + return (byte1 << 12) + (byte2 << 6) + (byte3); +} + + +unsigned int decode_4_byte_value(char* data) +{ + unsigned int byte1, byte2, byte3, byte4; + + byte1 = data[0] - 0x30; + byte2 = data[1] - 0x30; + byte3 = data[2] - 0x30; + byte4 = data[3] - 0x30; + + return (byte1 << 18) + (byte2 << 12) + (byte3 << 6) + (byte4); +} + + +void number_to_string(unsigned int num, char* dest, int length) +{ +#if defined(WIN32) + _snprintf(dest, length + 1, "%*d", length, num); +#else + snprintf(dest, length + 1, "%*d", length, num); +#endif + // Replace all leading spaces with '0' + for (int ii = 0; ii < length && dest[ii] == ' '; ii++) + dest[ii] = '0'; +} + + +/////////////////////////////////////////////////////////////////////////////// +// Sensor class +/////////////////////////////////////////////////////////////////////////////// + +// Public API +/////////////////////////////////////////////////////////////////////////////// + +Sensor::Sensor() + : port_(0), err_output_(std::cerr), scip_version_(2), verbose_(false), + enable_checksum_workaround_(false), ignore_unknowns_(false), + multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0), + resolution_(0.0), first_step_(0), last_step_(0), front_step_(0), + max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0), + wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0) +{ +} + + +Sensor::Sensor(std::ostream& err_output) + : port_(0), err_output_(err_output), scip_version_(2), verbose_(false), + enable_checksum_workaround_(false), ignore_unknowns_(false), + multiecho_mode_(ME_OFF), min_angle_(0.0), max_angle_(0.0), + resolution_(0.0), first_step_(0), last_step_(0), front_step_(0), + max_range_(0), time_resolution_(0), time_offset_(0), last_timestamp_(0), + wrap_count_(0), time_drift_rate_(0.0), time_skew_alpha_(0.0) +{ +} + + +Sensor::~Sensor() +{ + if(port_ != 0) + delete port_; +} + + +void Sensor::open(std::string port_options) +{ + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Creating and opening port using options: " << + port_options << '\n'; + } + port_ = flexiport::CreatePort(port_options); + port_->Open(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Connected using " << + port_->GetPortType() << " connection.\n"; + err_output_ << port_->GetStatus(); + } + port_->Flush(); + + // Figure out the SCIP version currently in use and switch to a higher one + // if possible + get_and_set_scip_version(); + // Get some values we need for providing default ranges + get_defaults(); +} + + +unsigned int Sensor::open_with_probing(std::string port_options) +{ + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Creating and opening port using options: " << port_options << + '\n'; + } + port_ = flexiport::CreatePort(port_options); + port_->Open(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Connected using " << + port_->GetPortType() << " connection.\n"; + err_output_ << port_->GetStatus(); + } + port_->Flush(); + + try + { + // Figure out the SCIP version currently in use and switch to a higher + // one if possible + get_and_set_scip_version(); + // Get some values we need for providing default ranges + get_defaults(); + } + catch(BaseError) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Failed to connect at the default baud rate.\n"; + } + if(port_->GetPortType() == "serial") + { + // Failed at the default baud rate, so try again at the other + // rates. Note that a baud rate of 750000 or 250000 doesn't appear + // to be supported on any common OS. + unsigned int const bauds[] = {500000, 115200, 57600, 38400, 19200}; + unsigned int const numBauds(5); + for (unsigned int ii = 0; ii < numBauds; ii++) + { + reinterpret_cast(port_)->SetBaudRate(bauds[ii]); + try + { + get_and_set_scip_version(); + get_defaults(); + // If the above two functions succeed, break out of the + // loop and be happy + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Connected at " << bauds[ii] << '\n'; + } + return bauds[ii]; + } + catch(BaseError) + { + if(ii == numBauds - 1) + { + // Last baud rate, give up and rethrow + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Failed to connect at any baud rate.\n"; + } + throw; + } + // Otherwise go around again + } + } + } + else + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Port is not serial, cannot probe.\n"; + } + throw; + } + } + + if(port_->GetPortType() == "serial") + return reinterpret_cast(port_)->GetBaudRate(); + else + return 0; +} + + +void Sensor::close() +{ + if(!port_) + throw CloseError(); + if(verbose_) + err_output_ << "Sensor::" << __func__ << "() Closing connection.\n"; + delete port_; + port_ = 0; +} + + +bool Sensor::is_open() const +{ + if(port_ != 0) + return port_->IsOpen(); + return false; +} + + +void Sensor::set_power(bool on) +{ + if(scip_version_ == 1) + { + if(on) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Turning laser on.\n"; + send_command("L", "1", 1, 0); + } + else + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Turning laser off.\n"; + send_command("L", "0", 1, 0); + } + skip_lines(1); + } + else if(scip_version_ == 2) + { + if(on) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Turning laser on.\n"; + send_command("BM", 0, 0, "02"); + } + else + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Turning laser off.\n"; + send_command("QT", 0, 0, "02"); + } + skip_lines(1); + } + else + throw UnknownScipVersionError(); +} + + +// This function assumes that both the port and the laser scanner are already +// set to the same baud. +void Sensor::set_baud(unsigned int baud) +{ + if(port_->GetPortType() != "serial") + throw NotSerialError(); + + char newBaud[13]; + memset(newBaud, 0, sizeof(char) * 13); + + if(baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 && + baud != 250000 && baud != 500000 && baud != 750000) + { + throw BaudrateError(baud); + } + number_to_string(baud, newBaud, 6); + + if(scip_version_ == 1) + { + // Send the command to change baud rate + send_command("S", newBaud, 13, 0); + skip_lines(1); + // Change the port's baud rate + reinterpret_cast(port_)->SetBaudRate(baud); + } + else if(scip_version_ == 2) + { + // Send the command to change baud rate + send_command("SS", newBaud, 6, "03"); + skip_lines(1); + // Change the port's baud rate + reinterpret_cast(port_)->SetBaudRate(baud); + } + else + throw UnknownScipVersionError(); +} + + +void Sensor::set_ip(IPAddr const& addr, IPAddr const& subnet, + IPAddr const& gateway) +{ + // Command is "$IP"(3) + IP(15) + ' ' + subnet(15) + ' ' + gateway(15), + // +1 for the null byte, but split two bytes off the front for sending. + // Hijack the send_command_ function, treating the rest of the command as + // parameters. + char const command[3] = "$I"; + std::stringstream params; + params << 'P'; + params << std::setw(3) << std::setfill('0'); + params << addr.first << '.' << addr.second << '.' << addr.third << '.' << + addr.fourth << ' '; + params << subnet.first << '.' << subnet.second << '.' << subnet.third << + '.' << subnet.fourth << ' '; + params << gateway.first << '.' << gateway.second << '.' << gateway.third << + '.' << gateway.fourth; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Setting IP information to $I" << params.str() << '\n'; + } + int status = send_command(&command[0], params.str().c_str(), 48, 0); + // Skip the extra line feed + skip_lines(1); + if(status != 0) + throw SetIPError(); +} + + +void Sensor::reset() +{ + if(scip_version_ == 1) + throw UnsupportedError(7); + else if(scip_version_ == 2) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Resetting laser.\n"; + } + send_command("RS", 0, 0, 0); + skip_lines(1); + } + else + throw UnknownScipVersionError(); +} + + +void Sensor::semi_reset() +{ + if(scip_version_ == 1) + throw UnsupportedError(35); + else if(scip_version_ == 2) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Resetting laser.\n"; + send_command("RS", 0, 0, 0); + skip_lines(1); + } + else + throw UnknownScipVersionError(); +} + + +void Sensor::set_motor_speed(unsigned int speed) +{ + if(scip_version_ == 1) + throw UnsupportedError(8); + else if(scip_version_ == 2) + { + // Sanity check the value + if(speed > 10 && speed != 99) + throw MotorSpeedError(); + char buffer[3]; + if(speed == 0) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Reseting motor speed to default.\n"; + } + buffer[0] = '0'; + buffer[1] = '0'; + buffer[2] = '\0'; + } + else if(speed == 99) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Reseting motor speed to default.\n"; + } + buffer[0] = '9'; + buffer[1] = '9'; + buffer[2] = '\0'; + } + else + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Setting motor speed to ratio " << speed << '\n'; + } + number_to_string(speed, buffer, 2); + } + send_command("CR", buffer, 2, "03"); + skip_lines(1); + } + else + throw UnknownScipVersionError(); +} + + +void Sensor::set_high_sensitivity(bool on) +{ + if(scip_version_ == 1) + throw UnsupportedError(10); + else if(scip_version_ == 2) + { + if(on) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Switching to high sensitivity.\n"; + send_command("HS", "1", 1, "02"); + } + else + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Switching to normal sensitivity.\n"; + } + send_command("HS", "0", 1, "02"); + } + skip_lines(1); + } + else + throw UnknownScipVersionError(); +} + + +void Sensor::get_sensor_info(SensorInfo& info) +{ + if(scip_version_ == 1) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Getting sensor information using SCIP version 1.\n"; + } + + info.set_defaults(); + + char buffer[SCIP1_LINE_LENGTH]; + memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH); + + send_command("V", 0, 0, 0); + // Get the vendor info line + read_line(buffer); + info.vendor = &buffer[5]; // Chop off the "VEND:" tag + // Get the product info line + read_line(buffer); + info.product = &buffer[5]; + // Only the URG-04LX supports SCIP1 + model_ = MODEL_URG04LX; + // Get the firmware line + read_line(buffer); + info.firmware = &buffer[5]; + // Get the protocol version line + read_line(buffer); + info.protocol = &buffer[5]; + // Get the serial number + read_line(buffer); + info.serial = &buffer[5]; + // Get either the status line or the end of message + read_line(buffer); + if(buffer[0] != '\0') + { + // Got a status line + info.sensor_diagnostic = &buffer[5]; + skip_lines(1); + } + + // Check the firmware version major number. If it's >=3 there is + // probably some extra info in the firmware line. + // eg: FIRM:3.1.04,07/08/02(20-4095[mm],240[deg],44-725[step],600[rpm]) + // Note that this example is right up against the maximum SCIP v1 line + // length of 64 bytes. + if(atoi(info.firmware.c_str()) >= 3) + { + if(verbose_) + err_output_ << "SCIP1 Firmware line for parsing: " << + info.firmware << '\n'; + // Now the fun part: parsing the line. It would be nice if we could + // use the POSIX regex functions, but since MS doesn't believe in + // POSIX we get to do it the hard way. + // Start by finding the first ( + char const* valueStart; + if((valueStart = strchr(info.firmware.c_str(), '(')) == 0) + { + // No bracket? Crud. Fail and use the hard-coded values from + // the manual. + info.calculate_values(); + } + // Now put it through sscanf and hope... + int aperture; + int numFound = sscanf(valueStart, + "(%d-%d[mm],%d[deg],%d-%d[step],%d[rpm]", &info.min_range, + &info.max_range, &aperture, &info.first_step, + &info.last_step, &info.speed); + if(numFound != 6) + { + // Didn't get enough values out, assume unknown format and fall + // back on the defaults + info.set_defaults(); + info.calculate_values(); + if(verbose_) + { + err_output_ << "Retrieved sensor info (hard-coded, not " + "enough values):\n"; + err_output_ << info.as_string(); + } + } + else + { + // Need to calculate stuff differently since it gave us an + // aperture value + info.resolution = DTOR(static_cast(aperture)) / + static_cast(info.last_step - info.first_step); + // Assume that the range is evenly spread + info.scanable_steps = info.last_step - info.first_step + 1; + info.front_step = info.scanable_steps / 2 + + info.first_step - 1; + info.min_angle = (static_cast(info.first_step) - + static_cast(info.front_step)) * info.resolution; + info.max_angle = (info.last_step - info.front_step) * + info.resolution; + + if(verbose_) + { + err_output_ << "Retrieved sensor info (from FIRM line):\n"; + err_output_ << info.as_string(); + } + } + } + else + { + // We're stuck with hard-coded defaults from the manual (already + // set earlier). + info.calculate_values(); + if(verbose_) + { + err_output_ << "Retrieved sensor info (hard-coded):\n"; + err_output_ << info.as_string(); + } + } + } + else if(scip_version_ == 2) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Getting sensor information using SCIP version 2.\n"; + } + + info.set_defaults(); + + char buffer[SCIP2_LINE_LENGTH]; + memset(buffer, 0, sizeof(char) * SCIP2_LINE_LENGTH); + + // We need to send three commands to get all the info we want: VV, PP + // and II + send_command("VV", 0, 0, 0); + while(read_line_with_check(buffer, -1, true) != 0) + process_vv_line(buffer, info); + + // Next up, PP + send_command("PP", 0, 0, 0); + while(read_line_with_check(buffer, -1, true) != 0) + process_pp_line(buffer, info); + + // Command II: Revenge of the Commands. + send_command("II", 0, 0, 0); + while(read_line_with_check(buffer, -1, true) != 0) + process_ii_line(buffer, info); + + enable_checksum_workaround_ = false; + + info.calculate_values(); + time_resolution_ = static_cast(info.time_resolution); + if(verbose_) + { + err_output_ << "Retrieved sensor info:\n"; + err_output_ << info.as_string(); + } + } + else + throw UnknownScipVersionError(); +} + + +unsigned int Sensor::get_time() +{ + return offset_timestamp(wrap_timestamp(get_raw_time())); +} + + +unsigned int Sensor::get_raw_time() +{ + if(scip_version_ == 1) + throw UnsupportedError(12); + else if(scip_version_ == 2) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Retrieving time from laser.\n"; + send_command("TM", "0", 1, 0); + send_command("TM", "1", 1, 0); + char buffer[7]; + read_line_with_check(buffer, 6); + send_command("TM", "2", 1, 0); + skip_lines(1); + // We need to decode the time value that's in the buffer + return decode_4_byte_value(buffer); + } + else + throw UnknownScipVersionError(); + + return 0; +} + + +long long Sensor::calibrate_time(unsigned int skew_sleep_time, + unsigned int samples) +{ + if(verbose_) + { + err_output_ << "Entering timing mode; start system time is " << + get_computer_time() << "ns.\n"; + } + enter_timing_mode(); + + // From A. Carballo, Y. Hara, H. Kawata, T. + // Yoshida, A. Ohya, S. Yuta, “Time synchronisation + // between SOKUIKI sensor and host computer using + // timestamps”, Proceedings of the JSME Conference on + // Robotics and Mechatronics ROBOMEC 2007, Akita, + // Japan, 2007, Paper 1P1-K05. + // + // Calibration is performed by calculating the offset between the laser's + // clock and the computer's clock. The algorithm is: + // Offset = Comp time before - + // (Laser time - (Comp time before - Comp time after) / 2) + // This is calculated samples times, and the median taken. + if(verbose_) + err_output_ << "Gathering " << samples << " offset values.\n"; + std::vector offsets; + for(unsigned int ii = 0; ii < samples; ii++) + { + unsigned long long end_time(0); + unsigned long long start_time = get_computer_time(); + unsigned long long laser_time = + wrap_timestamp(get_timing_mode_time(&end_time)) * 1e6; + offsets.push_back(start_time - + (laser_time - (end_time - start_time) / 2)); + if(verbose_) + { + err_output_ << "Offset #" << ii << " start_time = " << start_time; + err_output_ << "\tend_time = " << end_time; + err_output_ << "\tlaser_time = " << laser_time; + err_output_ << "\tCalculated offset = " << + (start_time - (laser_time - (end_time - start_time) / 2)) << + '\n'; + } + } + // Calculate the median offset + time_offset_ = median(offsets); + if(verbose_) + err_output_ << "Calculated offset is " << time_offset_ << '\n'; + + if(skew_sleep_time > 0) + { + // Sleep, then do it again to approximate a skew line + if(verbose_) + err_output_ << "Sleeping for " << skew_sleep_time << "s.\n"; +#if defined(WIN32) + DWORD sleep_time = skew_sleep_time * 1000; + Sleep(sleep_time); +#else + struct timespec sleep_time = {0, 0}; + sleep_time.tv_sec = skew_sleep_time; + nanosleep(&sleep_time, NULL); +#endif + + if(verbose_) + err_output_ << "Gathering " << samples << " offset values.\n"; + offsets.clear(); + for(unsigned int ii = 0; ii < samples; ii++) + { + unsigned long long end_time(0); + unsigned long long start_time = get_computer_time(); + unsigned long long laser_time = + wrap_timestamp(get_timing_mode_time(&end_time)) * 1e6; + offsets.push_back(start_time - + (laser_time - (end_time - start_time) / 2)); + } + // Calculate the median offset + long long offset2 = median(offsets); + if(verbose_) + { + err_output_ << "Calculated second offset is " << offset2 << + '\n'; + } + + // Approximate the line slope as (offset2 - offset1) / sleep_time + time_skew_alpha_ = (offset2 - time_offset_) / + static_cast(skew_sleep_time * 1e9); + if(verbose_) + { + err_output_ << "Calculated alpha is " << time_skew_alpha_ << '\n'; + } + } + + // All done. + if(verbose_) + err_output_ << "Leaving timing mode.\n"; + leave_timing_mode(); + return time_offset_; +} + + +unsigned int Sensor::get_ranges(ScanData& data, int start_step, + int end_step, unsigned int cluster_count) +{ + char buffer[11]; + memset(buffer, 0, sizeof(char) * 11); + + if(start_step < 0) + start_step = first_step_; + if(end_step < 0) + end_step = last_step_; + + unsigned int num_steps = (end_step - start_step + 1) / cluster_count; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " ranges between " << start_step << " and " << + end_step << " with a cluster count of " << cluster_count << + '\n'; + } + + if(scip_version_ == 1) + { + // Send the command to ask for the most recent range data from + // start_step to end_step + number_to_string(start_step, buffer, 3); + number_to_string(end_step, &buffer[3], 3); + number_to_string(cluster_count, &buffer[6], 2); + send_command("G", buffer, 8, 0); + // In SCIP1 mode we're going to get back 2-byte data + read_2_byte_range_data(data, num_steps); + } + else if(scip_version_ == 2) + { + // Send the command to ask for the most recent range data from + // start_step to end_step + number_to_string(start_step, buffer, 4); + number_to_string(end_step, &buffer[4], 4); + number_to_string(cluster_count, &buffer[8], 2); + if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) + send_command("HD", buffer, 10, 0); + else + send_command("GD", buffer, 10, 0); + // There will be a timestamp before the data (if there is data) + // Normally we would send 6 for the expected length, but we may get no + // timestamp back if there was no data. + if(read_line_with_check(buffer) == 0) + throw NoDataError(); + data.laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset(start_step); + data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_)); + // In SCIP2 mode we're going to get back 3-byte data because we're + // sending the GD command + read_3_byte_range_data(data, num_steps); + } + else + throw UnknownScipVersionError(); + + return data.ranges_length_; +} + + +unsigned int Sensor::get_ranges_by_angle(ScanData& data, double start_angle, + double end_angle, unsigned int cluster_count) +{ + // Calculate the given angles in steps, rounding towards front_step_ + int start_step, end_step; + start_step = angle_to_step(start_angle); + end_step = angle_to_step(end_angle); + + // Check the steps are within the allowable range + if(start_step < first_step_ || start_step > last_step_) + throw StartStepError(); + if(end_step < first_step_ || end_step > last_step_) + throw EndStepError(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Start angle " << + start_angle << " is step " << start_step << ", end angle " << + end_angle << " is step " << end_step << '\n'; + } + + // Get the data + return get_ranges(data, start_step, end_step, cluster_count); +} + + +unsigned int Sensor::get_ranges_intensities(ScanData& data, int start_step, + int end_step, unsigned int cluster_count) +{ + char buffer[11]; + memset(buffer, 0, sizeof(char) * 11); + + if(start_step < 0) + start_step = first_step_; + if(end_step < 0) + end_step = last_step_; + + unsigned int num_steps = (end_step - start_step + 1) / cluster_count; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " ranges between " << start_step << " and " << + end_step << " with a cluster count of " << cluster_count << + '\n'; + } + + if(scip_version_ == 1) + throw UnsupportedError(36); + else if(scip_version_ != 2) + throw UnknownScipVersionError(); + + // Send the command to ask for the most recent data from + // start_step to end_step + number_to_string(start_step, buffer, 4); + number_to_string(end_step, &buffer[4], 4); + number_to_string(cluster_count, &buffer[8], 2); + if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) + send_command("HE", buffer, 10, 0); + else + send_command("GE", buffer, 10, 0); + // There will be a timestamp before the data (if there is data) + // Normally we would send 6 for the expected length, but we may get no + // timestamp back if there was no data. + if(read_line_with_check(buffer) == 0) + throw NoDataError(); + data.laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset(start_step); + data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_)); + // In SCIP2 mode we're going to get back 3-byte data because we're + // sending the GE command + read_3_byte_range_data(data, num_steps); + + return data.ranges_length_; +} + + +unsigned int Sensor::get_ranges_intensities_by_angle(ScanData& data, + double start_angle, double end_angle, unsigned int cluster_count) +{ + // Calculate the given angles in steps, rounding towards front_step_ + int start_step, end_step; + start_step = angle_to_step(start_angle); + end_step = angle_to_step(end_angle); + + // Check the steps are within the allowable range + if(start_step < first_step_ || start_step > last_step_) + throw StartStepError(); + if(end_step < first_step_ || end_step > last_step_) + throw EndStepError(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Start angle " << + start_angle << " is step " << start_step << ", end angle " << + end_angle << " is step " << end_step << '\n'; + } + + // Get the data + return get_ranges_intensities(data, start_step, end_step, cluster_count); +} + + +unsigned int Sensor::get_new_ranges(ScanData& data, int start_step, + int end_step, unsigned int cluster_count) +{ + if(scip_version_ == 1) + throw UnsupportedError(16); + else if(scip_version_ != 2) + throw UnknownScipVersionError(); + + char buffer[14]; + memset(buffer, 0, sizeof(char) * 14); + + if(start_step < 0) + start_step = first_step_; + if(end_step < 0) + end_step = last_step_; + + unsigned int num_steps = (end_step - start_step + 1) / cluster_count; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " new ranges between " << start_step << " and " << + end_step << " with a cluster count of " << cluster_count << + '\n'; + } + + // Send the command to ask for the most recent range data from start_step to end_step + number_to_string(start_step, buffer, 4); + number_to_string(end_step, &buffer[4], 4); + number_to_string(cluster_count, &buffer[8], 2); + number_to_string(1, &buffer[10], 1); + number_to_string(1, &buffer[11], 2); + char command[3]; + if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) + command[0] = 'N'; + else + command[0] = 'M'; + command[1] = 'D'; + command[2] = '\0'; + send_command(command, buffer, 13, 0); + // Mx commands will perform a scan, then send the data prefixed with + // another command echo. + // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes) + char response[17]; + skip_lines(1); // End of the command echo message + read_line(response, 16); // Size is command(2)+params(13)+new line(1) + // Check the echo is correct + if(response[0] != command[0] || response[1] != command[1]) + throw CommandEchoError(command, response); + // Then compare the parameters + buffer[12] = '0'; // There will be zero scans remaining after this one + if(memcmp(&response[2], buffer, 13) != 0) + throw ParamEchoError(command); + // The next line should be the status line + read_line_with_check(response, 4); + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() " << command << " data prefix status: " << response[0] << + response[1] << '\n'; + } + // Check the status code is OK - should only get 99 here + if(response[0] != '9' || response[1] != '9') + { + // There is an extra line feed after an error status (signalling + // end of message) + skip_lines(1); + throw ResponseError(response, command); + } + + // Now the actual data will arrive + // There will be a timestamp before the data (if there is data) + // Normally we would send 6 for the expected length, but we may get no + // timestamp back if there was no data. + if(read_line_with_check(buffer) == 0) + throw NoDataError(); + data.laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset(start_step); + data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_)); + // In SCIP2 mode we're going to get back 3-byte data because we're + // sending the MD command + read_3_byte_range_data(data, num_steps); + + return data.ranges_length_; +} + + +unsigned int Sensor::get_new_ranges_by_angle(ScanData& data, + double start_angle, double end_angle, unsigned int cluster_count) +{ + if(scip_version_ == 1) + throw UnsupportedError(16); + + // Calculate the given angles in steps, rounding towards front_step_ + int start_step, end_step; + start_step = angle_to_step(start_angle); + end_step = angle_to_step(end_angle); + + // Check the steps are within the allowable range + if(start_step < first_step_ || start_step > last_step_) + throw StartStepError(); + if(end_step < first_step_ || end_step > last_step_) + throw EndStepError(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Start angle " << + start_angle << " is step " << start_step << ", end angle " << + end_angle << " is step " << end_step << '\n'; + } + + // Get the data + return get_new_ranges(data, start_step, end_step, cluster_count); +} + + +unsigned int Sensor::get_new_ranges_intensities(ScanData& data, + int start_step, int end_step, unsigned int cluster_count) +{ + if(scip_version_ == 1) + throw UnsupportedError(17); + else if(scip_version_ != 2) + throw UnknownScipVersionError(); + + char buffer[14]; + memset(buffer, 0, sizeof(char) * 14); + + if(start_step < 0) + start_step = first_step_; + if(end_step < 0) + end_step = last_step_; + + unsigned int num_steps = (end_step - start_step + 1) / cluster_count; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " new ranges and intensities between " << + start_step << " and " << end_step << + " with a cluster count of " << cluster_count << '\n'; + } + + // Send the command to ask for the most recent range data with + // intensity data from start_step to end_step + number_to_string(start_step, buffer, 4); + number_to_string(end_step, &buffer[4], 4); + number_to_string(cluster_count, &buffer[8], 2); + number_to_string(1, &buffer[10], 1); + number_to_string(1, &buffer[11], 2); + char command[3]; + if(model_ == MODEL_UXM30LXE && multiecho_mode_ != ME_OFF) + command[0] = 'N'; + else + command[0] = 'M'; + command[1] = 'E'; + command[2] = '\0'; + send_command(command, buffer, 13, 0); + // Mx commands will perform a scan, then send the data prefixed with + // another command echo. + // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes) + char response[17]; + skip_lines(1); // End of the command echo message + read_line(response, 16); // Size is command(2)+params(13)+new line(1) + // Check the echo is correct + if(response[0] != command[0] || response[1] != command[1]) + throw CommandEchoError(command, response); + // Then compare the parameters + buffer[12] = '0'; // There will be zero scans remaining after this one + if(memcmp(&response[2], buffer, 13) != 0) + throw ParamEchoError(command); + // The next line should be the status line + read_line_with_check(response, 4); + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() " << command << " data prefix status: " << response[0] << + response[1] << '\n'; + } + // Check the status code is OK - should only get 99 here + if(response[0] != '9' || response[1] != '9') + { + // There is an extra line feed after an error status (signalling + // end of message) + skip_lines(1); + throw ResponseError(response, command); + } + + // Now the actual data will arrive + // There will be a timestamp before the data (if there is data) + // Normally we would send 6 for the expected length, but we may get no + // timestamp back if there was no data. + if(read_line_with_check(buffer) == 0) + throw NoDataError(); + data.laser_time_ = decode_4_byte_value(buffer) + + step_to_time_offset(start_step); + data.system_time_ = offset_timestamp(wrap_timestamp(data.laser_time_)); + // In SCIP2 mode we're going to get back 3-byte data because we're + // sending the ME command + read_3_byte_range_and_intensity_data(data, num_steps); + + return data.ranges_length_; +} + + +unsigned int Sensor::get_new_ranges_intensities_by_angle(ScanData& data, + double start_angle, double end_angle, unsigned int cluster_count) +{ + if(scip_version_ == 1) + throw UnsupportedError(17); + + // Calculate the given angles in steps, rounding towards front_step_ + int start_step, end_step; + start_step = angle_to_step(start_angle); + end_step = angle_to_step(end_angle); + + // Check the steps are within the allowable range + if(start_step < first_step_ || start_step > last_step_) + throw StartStepError(); + if(end_step < first_step_ || end_step > last_step_) + throw EndStepError(); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Start angle " << + start_angle << " is step " << start_step << ", end angle " << + end_angle << " is step " << end_step << '\n'; + } + + // Get the data + return get_new_ranges_intensities(data, start_step, end_step, + cluster_count); +} + + +double Sensor::step_to_angle(unsigned int step) +{ + return (static_cast(step) - static_cast(front_step_)) * + resolution_; +} + + +unsigned int Sensor::angle_to_step(double angle) +{ + unsigned int result; + double resultF; + resultF = front_step_ + + (static_cast(angle) / static_cast(resolution_)); + // Round towards front_step_ so that the step values are always inside the + // angles given + if(resultF < front_step_) + result = static_cast(ceil(resultF)); + else + result = static_cast(floor(resultF)); + + return result; +} + + +// Private functions +/////////////////////////////////////////////////////////////////////////////// + +// Sometimes, just flushing isn't enough, as it appears the scanner will wait +// when the buffer gets full, then continue sending data. If we flush, we get +// rid of what was sent, and the scanner just sends more. This is a problem if +// we're trying to clear the result of a previous command. To get around this, +// keep flushing until the port reports there is no data left after a timeout. +// This shouldn't be called too much, as it introduces a delay as big as the +// timeout (which may be infinite). +void Sensor::clear_read_buffer() +{ + while(port_->BytesAvailableWait() > 0) + port_->Flush(); +} + + +// If expected_length is not -1, it should include the terminating line feed but +// not the 0 (although the buffer still has to include this). +// If expected_length is -1, this function expects buffer to be a certain length +// to allow up to the maximum line length to be read. See SCIP1_LINE_LENGTH and +// SCIP2_LINE_LENGTHr +// If fast is true, Read() will be called instead of ReadLine(), which should +// result in a faster, less CPU-intensive read. This is used, for example, when +// reading the range data. +// The line feed that terminates a line will be replaced with a 0. +// The return value is the number of bytes received, not including the 0 +// byte or the line feed. +int Sensor::read_line(char* buffer, int expected_length) +{ + ssize_t linelength = 0; + + if(expected_length == -1) + { + int maxlength = (scip_version_ == 1) ? + SCIP1_LINE_LENGTH : SCIP2_LINE_LENGTH; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading up to " << + maxlength << " bytes.\n"; + } + // We need to get at least 1 byte in a line: the line feed. + if((linelength = port_->ReadLine(buffer, maxlength)) < 0) + throw ReadError(0); + else if(linelength == 0) + throw ReadError(1); + // Replace the line feed with a 0 + buffer[linelength - 1] = '\0'; + } + else + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Reading exactly " << expected_length << " bytes.\n"; + } + // expected_length+1 for the 0 + if((linelength = port_->ReadLine(buffer, expected_length + 1)) < 0) + throw ReadError(0); + else if(linelength == 0) + throw ReadError(1); + else if(linelength < expected_length) + throw LineLengthError(linelength, expected_length); + // Replace the line feed with a 0 + buffer[linelength - 1] = '\0'; + } + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Read " << linelength << + " bytes.\n"; + err_output_ << "Sensor::" << __func__ << "() Line is '" << buffer << + "'\n"; + } + return linelength - 1; // Line feed not included +} + + +// This function will read a line and then calculate its checksum, comparing it +// with the checksum at the end of the line. The checksum will be removed +// (along with the semi-colon, if present). buffer and expected_length args are +// as for read_line(). +// If has_semicolon is true, the byte before the checksum is assumed to be the +// semi-colon separator and so not a part of the checksum. If it's not a +// semi-colon, an exception is thrown. +// Empty lines (i.e. a line that is just the line feed, as at the end of the +// message) will result in a return value of zero and no checksum check will be +// performed. Otherwise the number of actual data bytes (i.e. excluding the +// checksum and semicolon) will be returned. +// BIG NOTE: The UTM-30LX has a probable bug in its response to the II code +// whereby it does not include anything after and including "<-" in the +// checksum calculation. A workaround is enabled in this function when +// model_ is MODEL_UTM30LX. In this case, if the checksum fails normally, it +// scans the line for "<-" and recalculates the checksum on the bytes up to +// that point. This only happens in SCIP v2. +int Sensor::read_line_with_check(char* buffer, int expected_length, + bool has_semicolon) +{ + int linelength = read_line(buffer, expected_length); + if(scip_version_ == 1) + { + // No checksums in SCIP version 1 + return linelength; + } + + // If the line is empty, assume it was a line-feed message terminator, in + // which case there is no checksum to check. + if(linelength == 0) + return 0; + + // Ignore the checksum itself, and possibly a semicolon (read_line_ has + // already chopped off the line feed for us). + int bytesToConsider = linelength - 1 - (has_semicolon ? 1 : 0); + int checksumIndex = bytesToConsider + (has_semicolon ? 1 : 0); + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Considering " << + bytesToConsider << " bytes for checksum from a line length of " << + linelength << " bytes.\n"; + } + if(bytesToConsider < 1) + throw InsufficientBytesError(bytesToConsider, linelength); + + int checksum = 0; + try + { + checksum = confirm_checksum(buffer, bytesToConsider, + static_cast(buffer[checksumIndex])); + } + catch(ProtocolError& e) + { + if(model_ == MODEL_UTM30LX && enable_checksum_workaround_) + { + // Here comes the UTM-30LX workaround + char* hasComment = strstr(buffer, "<-"); + if(hasComment != 0) + { + int newBytesToConsider = hasComment - buffer; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Performing UTM-30LX II response " + "checksum workaround: trying with " << + newBytesToConsider << " bytes.\n"; + } + if(newBytesToConsider < 1) + { + throw InsufficientBytesError(newBytesToConsider, + linelength); + } + + checksum = confirm_checksum(buffer, newBytesToConsider, + static_cast(buffer[checksumIndex])); + } + else + // Workaround is disabled - rethrow + throw; + } + else + // Not a workaround-compatible error - rethrow + throw; + } + + // Null out the semi-colon (if there) and checksum + buffer[bytesToConsider] = '\0'; + + return bytesToConsider; +} + + +// Data blocks are treated as a special case of lines. This allows us to easily +// implement a faster read with less condition checks, as the format is more +// uniform. +// Returns true if the read data block was not a full data block, and thus is +// the end of the data. +bool Sensor::read_data_block(char* buffer, int& block_size) +{ + bool is_last(false); + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading exactly " << + DATA_BLOCK_LENGTH + 2 << " bytes.\n"; + } + // Read up to DATA_BLOCK_SIZE + 1 (checksum) + 1 (line feed) bytes, + // stopping as soon as a read puts a new line on the end + block_size = 0; + int read_goal = DATA_BLOCK_LENGTH + 2; + while (block_size < read_goal) + { + ssize_t bytes_read(0); + if((bytes_read = port_->Read(&buffer[block_size], + read_goal - block_size)) < 0) + { + throw ReadError(0); + } + else if(bytes_read == 0) + { + throw ReadError(1); + } + block_size += bytes_read; + if (buffer[block_size - 1] == '\n') + { + // New line = end of data line + break; + } + } + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Read " << block_size << + " bytes.\n"; + err_output_ << "Sensor::" << __func__ << "() Line is '" << buffer << + "'\n"; + } + // The data block should finish with one or two new lines. If it finishes + // with two, this is the last data block. + if (buffer[block_size - 1] != '\n') + { + throw ReadError(38); + } + buffer[block_size - 1] = '\0'; + block_size -= 1; // Remove the new line + if (buffer[block_size - 1] == '\n') + { + if (verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Found last block.\n"; + } + is_last = true; + buffer[block_size - 1] = '\0'; + block_size -= 1; // So it doesn't get used in the checksum calculation + } + // Check the checksum, which is the last byte in the block. + int bytes_to_consider = block_size - 1; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Considering " << + bytes_to_consider << " bytes for checksum from a line length of " << + block_size << " bytes.\n"; + } + if(bytes_to_consider < 1) + throw InsufficientBytesError(bytes_to_consider, block_size); + confirm_checksum(buffer, bytes_to_consider, + static_cast(buffer[bytes_to_consider])); + // Nullify the checksum + buffer[bytes_to_consider] = '\0'; + block_size -= 1; + + return is_last; +} + + +// Reads lines until the number specified has passed. +void Sensor::skip_lines(int count) +{ + if(verbose_) + err_output_ << "Sensor::" << __func__ << "() Skipping " << count << + " lines.\n"; + if(port_->SkipUntil(0x0A, count) < 0) + throw ReadError(18); +} + + +// Sends a command with optional parameters and checks that the echo of the +// command and parameters sent are correct, and that the returned status code +// is 0 or the first byte of extra_ok (for SCIP1), or 00, 99 or the first two +// bytes of extra_ok (for SCIP2). +// cmd must be a 1 byte string for SCIP1 and a 2-byte 0-terminated string +// for SCIP2. +// If param_length is 0, no parameters will be sent or expected in the reply. +// extra_ok must be a 1-byte string for SCIP1 and a 2-byte string for SCIP2. +// Return value is the status code returned for the command. +int Sensor::send_command(char const* cmd, char const* param, + int param_length, char const* extra_ok) +{ + int statusCode = -1; + char response[17]; + + // Flush first to clear out the dregs of any previous commands + port_->Flush(); + + if(scip_version_ == 1) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Writing in SCIP1 mode. Command is " << cmd[0] << + ", parameters length is " << param_length << '\n'; + } + // Write the command + if(port_->Write(cmd, 1) < 1) + throw WriteError(19); + if(param_length > 0) + { + if(port_->Write(param, param_length) < param_length) + throw WriteError(20); + } + if(port_->Write("\n", 1) < 1) + throw WriteError(21); + + // Read back the response (should get at least 4 bytes , possibly up to + // 16 including \n's depending on the parameters): cmd[0] params \n + // status \n + int statusIndex = 2 + param_length; + read_line(response, 2 + param_length); + read_line(&response[statusIndex], 2); + // First make sure that the echoed command matches + if(response[0] != cmd[0]) + { + char temp_cmd[2]; + temp_cmd[0] = cmd[0]; + temp_cmd[1] = '\0'; + char temp_echo[2]; + temp_echo[0] = response[0]; + temp_echo[1] = '\0'; + throw CommandEchoError(temp_cmd, temp_echo); + } + // Then compare the parameters + if(param_length > 0) + { + if(memcmp(&response[1], param, param_length) != 0) + { + char temp_cmd[2]; + temp_cmd[0] = cmd[0]; + temp_cmd[1] = '\0'; + throw ParamEchoError(temp_cmd); + } + } + // Next up, check the status byte + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Command response status: " << response[statusIndex] << + '\n'; + } + if(response[statusIndex] != '0') + { + if(extra_ok != 0) + { + if(response[statusIndex] != extra_ok[0]) + { + // There is an extra line feed after an error status + // (signalling end of message) + skip_lines(1); + throw Scip1ResponseError(response[statusIndex], cmd[0]); + } + } + else + { + // There is an extra line feed after an error status + // (signalling end of message) + skip_lines(1); + throw Scip1ResponseError(response[statusIndex], cmd[0]); + } + } + statusCode = atoi(&response[statusIndex]); + // All OK, data starts at beginning of port's buffer + } + else if(scip_version_ == 2) + { + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Writing in SCIP2 mode. Command is " << cmd << + ", parameters length is " << param_length << '\n'; + } + // Write the command + if(port_->Write(cmd, 2) < 2) + throw WriteError(19); + if(param_length > 0) + { + if(port_->Write(param, param_length) < param_length) + throw WriteError(20); + } + if(port_->Write("\n", 1) < 1) + throw WriteError(21); + + // Read back the command echo (minimum of 3 bytes, maximum of 16 bytes) + read_line(response, 3 + param_length); + // Check the echo is correct + if(response[0] != cmd[0] || response[1] != cmd[1]) + throw CommandEchoError(cmd, response); + // Then compare the parameters + if(param_length > 0) + { + if(memcmp(&response[2], param, param_length) != 0) + throw ParamEchoError(cmd); + } + + // The next line should be the status line + read_line_with_check(response, 4); + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Command response status: " << response[0] << response[1] << + '\n'; + } + // Check the status code is OK + response[2] = '\0'; + if(!(response[0] == '0' && response[1] == '0') && + !(response[0] == '9' && response[1] == '9')) + { + if(extra_ok != 0) + { + if(response[0] != extra_ok[0] || response[1] != extra_ok[1]) + { + // There is an extra line feed after an error status + // (signalling end of message) + skip_lines(1); + throw ResponseError(response, cmd); + } + } + else + { + // There is an extra line feed after an error status + // (signalling end of message) + skip_lines(1); + throw ResponseError(response, cmd); + } + } + statusCode = atoi(response); + // All OK, data starts at beginning of port's buffer + } + else + throw UnknownScipVersionError(); + + return statusCode; +} + + +/// Puts the laser into the timing mode. +void Sensor::enter_timing_mode() +{ + send_command("TM", "0", 1, 0); + skip_lines(1); +} + + +/// Take the laser out of timing mode. +void Sensor::leave_timing_mode() +{ + send_command("TM", "2", 1, 0); + skip_lines(1); +} + + +/// Get the timestamp from the laser. +/// If @ref reception_time is not 0, it will be filled with the time at +/// which data reception was completed. +unsigned int Sensor::get_timing_mode_time(unsigned long long* reception_time) +{ + // To get the most accurate result, we cannot do any processing while + // receiving. We want to know the time that reception finished as exactly + // as possible. To achieve this, we do not use send_command_, but instead + // send the command manually and receive the entire expected reply at once, + // then check/decode it later. + char response[17]; + if(port_->Write("TM1\n", 4) < 4) + throw WriteError(19); + ssize_t line_length = port_->Read(response, 16); + if(reception_time) + *reception_time = get_computer_time(); + // Process the response to confirm it is correct + if(line_length < 0) + throw ReadError(0); + else if(line_length == 0) + throw ReadError(1); + else if(line_length < 15) + throw LineLengthError(line_length, 15); + response[line_length - 1] = '\0'; + if(response[0] != 'T' || response[1] != 'M' || response[2] != '1') + throw CommandEchoError("TM", response); + response[7] = '\0'; + if(response[4] != '0' || response[5] != '0' || response[6] != 'P') + throw ResponseError(response, "TM"); + // Check the checksum on the time stamp is accurate + confirm_checksum(&response[8], 4, response[12]); + // Decode the time stamp + unsigned int timestamp = + decode_4_byte_value(&response[8]); + return timestamp; +} + + +/// Get the computer's time as accurately as possible. +unsigned long long Sensor::get_computer_time() +{ +#if defined(_POSIX_TIMERS) + struct timespec ts; + clock_gettime(CLOCK_REALTIME, &ts); + return ts.tv_sec * 1e9 + ts.tv_nsec; +#else +#if defined(WIN32) + SYSTEMTIME sys_time; + GetSystemTime(&sys_time); + return sys_time.wSecond * 1e9 + sys_time.wMilliseconds * 1e3; +#else + struct timeval tv; + gettimeofday(&tv, 0); + return tv.tv_sec * 1e9 + tv.tv_usec * 1e3; +#endif +#endif +} + + +/// timestamp must be in milliseconds. The result is in milliseconds. +unsigned int Sensor::wrap_timestamp(unsigned int timestamp) +{ + if(timestamp < last_timestamp_) + { + wrap_count_++; + } + last_timestamp_ = timestamp; + + return timestamp + wrap_count_ * 0x01000000; // 24-bit value + 1 +} + + +/// timestamp must be in milliseconds. The result is in nanoseconds. +unsigned long long Sensor::offset_timestamp(unsigned int timestamp) +{ + return ((1 - time_drift_rate_) * timestamp * 1e6 + time_offset_) / + (1 - time_skew_alpha_); +} + + +unsigned int Sensor::step_to_time_offset(int start_step) +{ + if(start_step < 0) + return first_step_ * time_resolution_; + else + return start_step * time_resolution_; +} + + +/// Search a string for the laser's model. +void Sensor::find_model(char const* buffer) +{ + if(strstr(buffer, "URG-04LX") != 0) + model_ = MODEL_URG04LX; + else if(strstr(buffer, "UBG-04LX") != 0) + model_ = MODEL_UBG04LXF01; + else if(strstr(buffer, "UHG-08LX") != 0) + model_ = MODEL_UHG08LX; + else if(strstr(buffer, "UTM-30LX") != 0) + { + model_ = MODEL_UTM30LX; + // Also enable the work around for a checksum problem in this + // model. + enable_checksum_workaround_ = true; + } + else if(strstr(buffer, "UXM-30LX") != 0) + model_ = MODEL_UXM30LXE; + else + model_ = MODEL_UNKNOWN; +} + + +void Sensor::get_and_set_scip_version() +{ + bool scip2Failed = false; + + + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Testing SCIP protocol version.\n"; + // Try SCIP version 2 first by sending an info command + try + { + send_command("VV", 0, 0, 0); + } + catch(BaseError) + { + // That didn't work too well... + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Initial SCIP version 2 test failed.\n"; + scip2Failed = true; + } + + if(scip2Failed) + { + // Currently using SCIP version 1 + // Get the firmware version and check if we can move to SCIP version 2 + scip_version_ = 1; + + port_->Flush(); + try + { + send_command("V", 0, 0, 0); + } + catch(BaseError) + { + throw ScipVersionError(); + } + // Skip the vendor and product info + skip_lines(2); + // Get the firmware line + char buffer[SCIP1_LINE_LENGTH]; + memset(buffer, 0, sizeof(char) * SCIP1_LINE_LENGTH); + read_line(buffer); + + if(strncmp(buffer, "FIRM:", 5) != 0) + throw MissingFirmSpecError(); + // Pull out the major version number + // Note that although lasers such as the UTM-30LX appear to use a + // different firmware version format, that doesn't matter because they + // don't support SCIP v1 and so shouldn't get to this point anyway - if + // they do, it's an uncaught error. + int majorVer = strtol(&buffer[5], 0, 10); + if(errno == ERANGE) + throw FirmwareError(); + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Firmware major version is " << majorVer << + '\n'; + } + // Dump the rest of the V command result (one of these will be the + // empty last line) + skip_lines(3); + + // If the firmware version is less than 3, we're stuck with SCIP + // version 1. + if(majorVer < 3) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Firmware does not support SCIP version 2; using SCIP " + "version 1.\n"; + return; + } + // Otherwise we can try SCIP version 2 + else + { + port_->Flush(); + // We'll hijack the send_command_ function a bit here. Normally it + // takes 1-byte commands, (we're currently using SCIP version 1, + // remember), but the command to change to SCIP version 2 is 7 + // bytes long (why did they have to do it that way?). So send the + // first byte as the command and the other 6 as parameters. + try + { + send_command("S", "CIP2.0", 6, 0); + } + catch(BaseError) + { + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Could not change to SCIP version 2; using SCIP " + "version 1.\n"; + return; + } + // There'll be a trailing line on the end + skip_lines(1); + + // Changed to SCIP version 2 + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Using SCIP version 2.\n"; + scip_version_ = 2; + return; + } + } + else + { + // Currently using SCIP version 2 + scip_version_ = 2; + + // Dump the rest of the result + skip_lines(6); + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Using SCIP version 2.\n"; + return; + } + + // Fallback case if didn't find a good SCIP version and return above + throw ScipVersionError(); +} + + +void Sensor::get_defaults() +{ + if(verbose_) + err_output_ << "Sensor::" << __func__ << + "() Getting default values.\n"; + + // Get the laser's info + SensorInfo info; + get_sensor_info(info); + + min_angle_ = info.min_angle; + max_angle_ = info.max_angle; + resolution_ = info.resolution; + first_step_ = info.first_step; + last_step_ = info.last_step; + front_step_ = info.front_step; + max_range_ = info.max_range; + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Got default values: " << min_angle_ << " " << max_angle_ << + " " << resolution_ << " " << first_step_ << " " << last_step_ << + " " << front_step_ << " " << max_range_ << '\n'; + } +} + + +void Sensor::process_vv_line(char const* buffer, SensorInfo& info) +{ + if(strncmp(buffer, "VEND", 4) == 0) + info.vendor = &buffer[5]; // Vendor info, minus the "VEND:" tag + else if(strncmp(buffer, "PROD", 4) == 0) + { + info.product = &buffer[5]; // Product info + // Find the product model + find_model(&buffer[5]); + info.detected_model = model_; + } + else if(strncmp(buffer, "FIRM", 4) == 0) + info.firmware = &buffer[5]; // Firmware version + else if(strncmp(buffer, "PROT", 4) == 0) + info.protocol = &buffer[5]; // Protocol version + else if(strncmp(buffer, "SERI", 4) == 0) + info.serial = &buffer[5]; // Serial number + else if(!ignore_unknowns_) + throw UnknownLineError(buffer); +} + + +void Sensor::process_pp_line(char const* buffer, SensorInfo& info) +{ + if(strncmp(buffer, "MODL", 4) == 0) + info.model = &buffer[5]; // Model + // On to the fun ones that require parsing + else if(strncmp(buffer, "DMIN", 4) == 0) + info.min_range = atoi(&buffer[5]); + else if(strncmp(buffer, "DMAX", 4) == 0) + info.max_range = atoi(&buffer[5]); + else if(strncmp(buffer, "ARES", 4) == 0) + info.steps = atoi(&buffer[5]); + else if(strncmp(buffer, "AMIN", 4) == 0) + info.first_step = atoi(&buffer[5]); + else if(strncmp(buffer, "AMAX", 4) == 0) + info.last_step = atoi(&buffer[5]); + else if(strncmp(buffer, "AFRT", 4) == 0) + info.front_step = atoi(&buffer[5]); + else if(strncmp(buffer, "SCAN", 4) == 0) + info.standard_speed = atoi(&buffer[5]); + /* No example in the manual and sensor with support for this has not + * arrived yet, so don't know what to look for. + else if(strncmp(buffer, "", 4) == 0) + { + if(strstr(buffer, "CCW") != 0) + info.rot_dir = COUNTERCLOCKWISE; + else + info.rot_dir = CLOCKWISE; + }*/ + else if(!ignore_unknowns_) + throw UnknownLineError(buffer); +} + + +void Sensor::process_ii_line(char const* buffer, SensorInfo& info) +{ + if(strncmp(buffer, "MODL", 4) == 0) + // Do nothing here - we already know this value from PP + return; + else if(strncmp(buffer, "LASR", 4) == 0) + { + if(strncmp(&buffer[5], "OFF", 3) == 0) + info.power = false; + else + info.power = true; + } + else if(strncmp(buffer, "SCSP", 4) == 0) + { + if(strncmp(&buffer[5], "Initial", 7) == 0) + { + // Unchanged motor speed + if(sscanf(buffer, "SCSP:%*7s(%d[rpm]", &info.speed) != 1) + { + throw ParseError(buffer, "Motor speed"); + } + info.speed_level = 0; + } + else + { + // Changed motor speed, format is: + // %([rpm]) + if(sscanf(buffer, "SCSP:%hd%%%*4s(%d[rpm]", &info.speed_level, + &info.speed) != 2) + { + throw ParseError(buffer, "Motor speed"); + } + } + } + else if(strncmp(buffer, "MESM", 4) == 0) + info.measure_state = &buffer[5]; + else if(strncmp(buffer, "SBPS", 4) == 0) + { + if(strncmp(&buffer[5], "USB only", 8) == 0 || + strncmp(&buffer[5], "USB Full Speed", 14) == 0) + { + // No baud rate for USB-only devices such as the UHG-08LX + info.baud = 0; + } + else if(sscanf(buffer, "SBPS:%d[bps]", &info.baud) != 1) + throw ParseError(buffer, "Baud rate"); + } + else if(strncmp(buffer, "TIME", 4) == 0) + { + if(sscanf(buffer, "TIME:%x", &info.time) != 1) + throw ParseError(buffer, "Timestamp"); + } + else if(strncmp(buffer, "STAT", 4) == 0) + info.sensor_diagnostic = &buffer[5]; + else if(!ignore_unknowns_) + throw UnknownLineError(buffer); +} + + +/// Combines up to three values from an echo buffer into a single value based +/// on the setting of multiecho_mode_. +uint32_t Sensor::process_echo_buffer(int const* buffer, int num_echos) +{ + uint32_t sum = 0; + switch(multiecho_mode_) + { + case ME_FRONT: + return buffer[0]; + break; + case ME_MIDDLE: + if(num_echos == 3) + return buffer[1]; + else + return buffer[0]; + break; + case ME_REAR: + return buffer[num_echos -1 ]; + break; + case ME_AVERAGE: + for (int ii = 0; ii < num_echos; ii++) + sum += buffer[ii]; + sum /= static_cast(num_echos); + return sum; + break; + case ME_OFF: + default: + return buffer[0]; + break; + } +} + + +void Sensor::read_2_byte_range_data(ScanData& data, unsigned int num_steps) +{ + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " ranges.\n"; + } + + // This will automatically take care of whether it actually needs to + // (re)allocate or not. + data.allocate_data(num_steps); + data.model_ = model_; + data.error_ = false; + + // 2 byte data is easy since it fits neatly in a 64-byte block + char buffer[SCIP2_LINE_LENGTH]; + unsigned int current_step(0); + int numBytesInLine(0); + bool done(false); + while(!done) + { + // Read a line of data + done = read_data_block(buffer, numBytesInLine); + // Check if we've reached the end of the data + if(numBytesInLine == 0) + { + err_output_ << "numBytesInLine is zero!\n"; + } + // Process pairs of bytes until we encounter the end of the line + for (int ii = 0; ii < numBytesInLine; ii += 2, current_step++) + { + if(buffer[ii] == '\n' || buffer[ii + 1] == '\n') + { + // Line feed in the middle of a data block? Why? + throw MisplacedLineFeedError(); + } + data.write_range(current_step, decode_2_byte_value(&buffer[ii])); + } + // End of this line. Go around again. + } + + if(verbose_) + err_output_ << "Sensor::" << __func__ << "() Read " << + current_step << " ranges.\n"; + if(current_step != num_steps) + throw DataCountError(); +} + + +void Sensor::read_3_byte_range_data(ScanData& data, unsigned int num_steps) +{ + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " ranges.\n"; + if(multiecho_mode_ != ME_OFF) + { + err_output_ << "Sensor::" << __func__ << + "() Multi-echo mode is set to " << + multiecho_mode_to_string(multiecho_mode_) << '\n'; + } + } + + // This will automatically take care of whether it actually needs to + // (re)allocate or not. + data.allocate_data(num_steps); + data.model_ = model_; + data.error_ = false; + + // 3 byte data is a pain because it crosses the line boundary, it may + // overlap by 0, 1 or 2 bytes + char buffer[SCIP2_LINE_LENGTH]; + unsigned int current_step(0); + int numBytesInLine(0), split_count(0); + char split_value[3]; + int echo_buffer[3] = {-1, -1, -1}; + int echo_buf_ind(0); + bool done(false); + while(!done) + { + // Read a line of data + done = read_data_block(buffer, numBytesInLine); + // Check if we've reached the end of the data + if(numBytesInLine == 0) + { + err_output_ << "numBytesInLine is zero!\n"; + } + // Process triplets of bytes until we encounter or overrun the end of + // the line + for (int ii = 0; ii < numBytesInLine;) + { + if(buffer[ii] == '\n' || buffer[ii + 1] == '\n') + { + // Line feed in the middle of a line? Why? + throw MisplacedLineFeedError(); + } + if(split_count == 0) + { + // Start of a value. Decide where to store the next value, and + // if the previous is complete. + if(buffer[ii] == '&') + { + // Next echo + echo_buf_ind++; + ii++; + } + else if(echo_buffer[0] != -1) + { + // Not the first value, so deal with the previous + data.write_range(current_step, + process_echo_buffer(echo_buffer, + echo_buf_ind + 1)); + if(data.ranges_) + { + if(data.ranges_[current_step] > max_range_) + { + err_output_ << "WARNING: Sensor::" << __func__ << + "() Value at step " << current_step << + " beyond maximum range: " << + data.ranges_[current_step] << '\n'; + } + } + current_step++; + echo_buf_ind = 0; + echo_buffer[0] = -1; + } + } + if(ii == numBytesInLine - 2) // Short 1 byte + { + split_value[0] = buffer[ii]; + split_value[1] = buffer[ii + 1]; + // Will be reset on the next iteration, after it's used + split_count = 1; + ii += 2; + } + else if(ii == numBytesInLine - 1) // Short 2 bytes + { + split_value[0] = buffer[ii]; + // Will be reset on the next iteration, after it's used + split_count = 2; + ii += 1; + } + else + { + if(split_count == 1) + { + split_value[2] = buffer[ii++]; + echo_buffer[echo_buf_ind] = + decode_3_byte_value(split_value); + } + else if(split_count == 2) + { + split_value[1] = buffer[ii++]; + split_value[2] = buffer[ii++]; + echo_buffer[echo_buf_ind] = + decode_3_byte_value(split_value); + } + else + { + echo_buffer[echo_buf_ind] = + decode_3_byte_value(&buffer[ii]); + ii += 3; + } + split_count = 0; // Reset this here now that it's been used + } + } + // End of this line. Go around again. + } + // Last little bit of data + if(echo_buffer[0] != -1) + { + // Not the first value, so deal with the previous + data.write_range(current_step, + process_echo_buffer(echo_buffer, echo_buf_ind + 1)); + if(data.ranges_) + { + if(data.ranges_[current_step] > max_range_) + { + err_output_ << "WARNING: Sensor::" << __func__ << + "() Value at step " << current_step << + " beyond maximum range: " << + data.ranges_[current_step] << '\n'; + } + } + current_step++; + } + + if(verbose_) + err_output_ << "Sensor::" << __func__ << "() Read " << + current_step << " ranges.\n"; + if(current_step != num_steps) + throw DataCountError(); +} + + +void Sensor::read_3_byte_range_and_intensity_data(ScanData& data, + unsigned int num_steps) +{ + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Reading " << + num_steps << " ranges and intensities.\n"; + if(multiecho_mode_ != ME_OFF) + { + err_output_ << "Sensor::" << __func__ << + "() Multi-echo mode is set to " << + multiecho_mode_to_string(multiecho_mode_) << '\n'; + } + } + + // This will automatically take care of whether it actually needs to + // (re)allocate or not. + data.allocate_data(num_steps, true); + data.model_ = model_; + + // 3 byte data is a pain because it crosses the line boundary, it may + // overlap by 0, 1 or 2 bytes + char buffer[SCIP2_LINE_LENGTH]; + unsigned int current_range(0), current_intensity(0); + int numBytesInLine(0), split_count(0); + char split_value[3]; + bool nextIsIntensity(false); + int echo_buffer[3] = {-1, -1, -1}; + int echo_buf_ind(0); + bool done(false); + while(!done) + { + // Read a line of data + done = read_data_block(buffer, numBytesInLine); + // Check if we've reached the end of the data + if(numBytesInLine == 0) + { + err_output_ << "numBytesInLine is zero!\n"; + } + // Process triplets of bytes until we encounter or overrun the end of + // the line + for (int ii = 0; ii < numBytesInLine;) + { + if(buffer[ii] == '\n' || buffer[ii + 1] == '\n') + { + // Line feed in the middle of a line? Why? + throw MisplacedLineFeedError(); + } + if(split_count == 0) + { + // Start of a value. Decide where to store the next value, and + // if the previous is complete. + if(buffer[ii] == '&') + { + // Next echo + echo_buf_ind++; + ii++; + } + else if(echo_buffer[0] != -1) + { + // Not the first value, so deal with the previous + if(nextIsIntensity) + { + data.write_intensity(current_intensity, + process_echo_buffer(echo_buffer, + echo_buf_ind + 1)); + } + else + { + data.write_range(current_range, + process_echo_buffer(echo_buffer, + echo_buf_ind + 1)); + } + echo_buf_ind = 0; + echo_buffer[0] = -1; + if(data.ranges_) + { + if(data.ranges_[current_range] > max_range_ && + !nextIsIntensity) + { + err_output_ << "WARNING: Sensor::" << __func__ << + "() Value at step " << current_range << + " beyond maximum range: " << + data.ranges_[current_range] << " (raw bytes: "; + if(split_count != 0) + err_output_ << split_value[0] << split_value[1] << + split_value[2] << ")\n"; + else + err_output_ << buffer[0] << buffer[1] << buffer[2] << + ")\n"; + } + } + if(nextIsIntensity) + current_intensity++; + else + current_range++; + // Alternate between range and intensity values + nextIsIntensity = !nextIsIntensity; + } + } + if(ii == numBytesInLine - 2) // Short 1 byte + { + split_value[0] = buffer[ii]; + split_value[1] = buffer[ii + 1]; + // Will be reset on the next iteration, after it's used + split_count = 1; + ii += 2; + } + else if(ii == numBytesInLine - 1) // Short 2 bytes + { + split_value[0] = buffer[ii]; + // Will be reset on the next iteration, after it's used + split_count = 2; + ii += 1; + } + else + { + if(split_count == 1) + { + split_value[2] = buffer[ii++]; + echo_buffer[echo_buf_ind] = + decode_3_byte_value(split_value); + } + else if(split_count == 2) + { + split_value[1] = buffer[ii++]; + split_value[2] = buffer[ii++]; + echo_buffer[echo_buf_ind] = + decode_3_byte_value(split_value); + } + else + { + echo_buffer[echo_buf_ind] = + decode_3_byte_value(&buffer[ii]); + ii += 3; + } + // Reset this here now that it's been used + split_count = 0; + } + } + // End of this line. Go around again. + } + // The last piece should always be an intensity value (if it isn't + // then the data count won't add up and an error will be thrown + // below anyway). + if(echo_buffer[0] != -1) + { + assert(nextIsIntensity == true); + data.write_intensity(current_intensity, + process_echo_buffer(echo_buffer, echo_buf_ind + 1)); + current_intensity++; + } + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << "() Read " << + current_range << " ranges and " << current_intensity << + " intensities (expected " << num_steps << ").\n"; + } + if(current_range != num_steps || current_intensity != num_steps) + throw DataCountError(); +} + + +int Sensor::confirm_checksum(char const* buffer, int length, + int expected_sum) +{ + int checksum = 0; + // Start by adding the byte values + for (int ii = 0; ii < length; ii++) + checksum += buffer[ii]; + // Take the lowest 6 bits + checksum &= 0x3F; + // Add 0x30 + checksum += 0x30; + + if(verbose_) + { + err_output_ << "Sensor::" << __func__ << + "() Calculated checksum = " << checksum << " (" << + static_cast(checksum) << "), given checksum = " << + expected_sum << " (" << static_cast (expected_sum) << + ")\n"; + } + if(checksum != expected_sum) + throw ChecksumError(expected_sum, checksum); + + return checksum; +} + +}; // namespace hokuyoaist + diff --git a/src/sensor_info.cpp b/src/sensor_info.cpp new file mode 100644 index 0000000..852476e --- /dev/null +++ b/src/sensor_info.cpp @@ -0,0 +1,174 @@ +/* HokuyoAIST + * + * Implementation of the sensor information object. + * + * Copyright 2008-2011 Geoffrey Biggs geoffrey.biggs@aist.go.jp + * RT-Synthesis Research Group + * Intelligent Systems Research Institute, + * National Institute of Advanced Industrial Science and Technology (AIST), + * Japan + * All rights reserved. + * + * This file is part of HokuyoAIST. + * + * HokuyoAIST is free software; you can redistribute it and/or modify it + * under the terms of the GNU Lesser General Public License as published + * by the Free Software Foundation; either version 2.1 of the License, + * or (at your option) any later version. + * + * HokuyoAIST is distributed in the hope that it will be useful, but + * WITHOUT ANY WARRANTY; without even the implied warranty of + * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU + * Lesser General Public License for more details. + * + * You should have received a copy of the GNU Lesser General Public + * License along with HokuyoAIST. If not, see + * . + */ + +#include +#include + +#include + +using namespace hokuyoaist; + +/////////////////////////////////////////////////////////////////////////////// +// SensorInfo class +/////////////////////////////////////////////////////////////////////////////// + +SensorInfo::SensorInfo() + : min_range(0), max_range(0), steps(0), first_step(0), last_step(0), + front_step(0), standard_speed(0), rot_dir(COUNTERCLOCKWISE), power(false), + speed(0), speed_level(0), baud(0), time(0), min_angle(0.0), max_angle(0.0), + resolution(0.0), time_resolution(0), scanable_steps(0), max_step(0), + detected_model(MODEL_UNKNOWN) +{ +} + + +// Set various known values based on what the manual says +void SensorInfo::set_defaults() +{ + switch(detected_model) + { + case MODEL_URG04LX: + min_range = 20; + max_range = 4095; + steps = 1024; + first_step = 44; + last_step = 725; + front_step = 384; + max_step = 768; + break; + case MODEL_UBG04LXF01: + min_range = 20; + max_range = 4095; + steps = 1024; + first_step = 44; + last_step = 725; + front_step = 384; + max_step = 768; + break; + case MODEL_UHG08LX: + min_range = 20; + max_range = 8000; + steps = 1024; + first_step = 0; + last_step = 768; + front_step = 384; + max_step = 768; + break; + case MODEL_UTM30LX: + min_range = 20; + max_range = 60000; + steps = 1440; + first_step = 0; + last_step = 1080; + front_step = 540; + max_step = 1080; + break; + case MODEL_UXM30LXE: + min_range = 20; + max_range = 60000; + steps = 1440; + first_step = 0; + last_step = 760; + front_step = 380; + max_step = 760; + break; + case MODEL_UNKNOWN: + default: + // Use the URG-04LX settings + min_range = 20; + max_range = 4095; + steps = 1024; + first_step = 44; + last_step = 725; + front_step = 384; + max_step = 768; + break; + } +} + + +void SensorInfo::calculate_values() +{ + resolution = DTOR(360.0) / steps; + // If any of the steps are beyond INT_MAX, we have problems. + // We also have an incredibly high-resolution sensor. + min_angle = (static_cast(first_step) - static_cast(front_step)) * + resolution; + max_angle = (static_cast(last_step) - static_cast(front_step)) * + resolution; + scanable_steps = last_step - first_step + 1; + // Calculate the time taken for a single scan. + if(speed == 0) + { + time_resolution = 60000.0 / 600.0; + } + else + { + // 60000 = milliseconds in a minute + // speed is in RPM + time_resolution = 60000.0 / speed; + } + time_resolution /= static_cast(steps); +} + + +std::string SensorInfo::as_string() +{ + std::stringstream ss; + + ss << "Vendor: " << vendor << '\n'; + ss << "Product: " << product << '\n'; + ss << "Identified as: " << model_to_string(detected_model) << '\n'; + ss << "Firmware: " << firmware << '\n'; + ss << "Protocol: " << protocol << '\n'; + ss << "Serial: " << serial << '\n'; + ss << "Model: " << model << '\n'; + + ss << "Minimum range: " << min_range << "mm\tMaximum range: " << max_range << + "mm\n"; + ss << "Steps in 360 degrees: " << steps << "\tScanable steps: " << + scanable_steps << '\n'; + ss << "First step: " << first_step << "\tFront step: " << front_step << + "\tLast step: " << last_step << "\tMax step: " << max_step << '\n'; + ss << "Resolution: " << resolution << " radians/step\n"; + ss << "Minimum angle: " << min_angle << " radians\tMaximum angle: " << + max_angle << " radians\n"; + ss << "Standard motor speed: " << standard_speed << "rpm\n"; + ss << "Rotation direction: " << rot_dir_to_string(rot_dir) << '\n'; + + ss << "Power status: " << (power ? "On" : "Off") << + "\tMeasurement state: " << measure_state << '\n'; + ss << "Motor speed: " << speed << "rpm (level " << speed_level << + ")\tBaud rate: " << baud << "bps\n"; + ss << "Time stamp: " << time << "ms\n"; + ss << "Time between scan points: " << time_resolution << "ms\n"; + ss << "Sensor diagnostic: " << sensor_diagnostic << '\n'; + + return ss.str(); +} +