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();
+}
+