├── AUTHORS ├── CHANGELOG.rst ├── CMakeLists.txt ├── COPYING ├── README ├── THANKS ├── c++ ├── drivers │ ├── ld │ │ └── sickld │ │ │ ├── .deps │ │ │ ├── SickLD.Plo │ │ │ ├── SickLDBufferMonitor.Plo │ │ │ └── SickLDMessage.Plo │ │ │ ├── SickLD.cc │ │ │ ├── SickLDBufferMonitor.cc │ │ │ └── SickLDMessage.cc │ ├── lms1xx │ │ └── sicklms1xx │ │ │ ├── .deps │ │ │ ├── SickLMS1xx.Plo │ │ │ ├── SickLMS1xxBufferMonitor.Plo │ │ │ └── SickLMS1xxMessage.Plo │ │ │ ├── SickLMS1xx.cc │ │ │ ├── SickLMS1xxBufferMonitor.cc │ │ │ └── SickLMS1xxMessage.cc │ └── lms2xx │ │ └── sicklms2xx │ │ ├── .deps │ │ ├── SickLMS2xx.Plo │ │ ├── SickLMS2xxBufferMonitor.Plo │ │ └── SickLMS2xxMessage.Plo │ │ ├── SickLMS2xx.cc │ │ ├── SickLMS2xxBufferMonitor.cc │ │ └── SickLMS2xxMessage.cc └── examples │ ├── ld │ ├── ld_config │ │ ├── Doxyfile │ │ ├── conf │ │ │ └── sickld.conf │ │ ├── doxygen-html-header │ │ └── src │ │ │ ├── .deps │ │ │ ├── ConfigFile.Po │ │ │ └── main.Po │ │ │ ├── ConfigFile.cpp │ │ │ ├── ConfigFile.h │ │ │ └── main.cc │ ├── ld_more_config │ │ └── src │ │ │ ├── .deps │ │ │ └── main.Po │ │ │ └── main.cc │ ├── ld_multi_sector │ │ └── src │ │ │ ├── .deps │ │ │ └── main.Po │ │ │ └── main.cc │ └── ld_single_sector │ │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms1xx │ └── lms1xx_simple_app │ │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ └── lms2xx │ ├── lms2xx_config │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_mean_values │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_partial_scan │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_real_time_indices │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_set_variant │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_simple_app │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ ├── lms2xx_stream_range_and_reflect │ ├── README │ └── src │ │ ├── .deps │ │ └── main.Po │ │ └── main.cc │ └── lms2xx_subrange │ ├── README │ └── src │ ├── .deps │ └── main.Po │ └── main.cc ├── doxygen.cfg ├── include └── sicktoolbox │ ├── SickBufferMonitor.hh │ ├── SickConfig.hh │ ├── SickException.hh │ ├── SickLD.hh │ ├── SickLDBufferMonitor.hh │ ├── SickLDMessage.hh │ ├── SickLDUtility.hh │ ├── SickLIDAR.hh │ ├── SickLMS1xx.hh │ ├── SickLMS1xxBufferMonitor.hh │ ├── SickLMS1xxMessage.hh │ ├── SickLMS1xxUtility.hh │ ├── SickLMS2xx.hh │ ├── SickLMS2xxBufferMonitor.hh │ ├── SickLMS2xxMessage.hh │ ├── SickLMS2xxUtility.hh │ └── SickMessage.hh ├── manuals ├── sicktoolbox-RS-422.pdf └── sicktoolbox-quickstart.pdf └── package.xml /AUTHORS: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------- 2 | ------------------------------------------------------------------- 3 | The Sick LIDAR Matlab/C++ Toolbox 4 | ------------------------------------------------------------------- 5 | 6 | The Sick LIDAR Matlab/C++ Toolbox was originally developed 7 | and is currently maintained by Jason Derenick and Thomas Miller 8 | at Lehigh University. It is branched from the source code used 9 | by the Ben Franklin Racing Team, whose robot car, "Little Ben", 10 | was only one of six to successfully finish the 2007 DARPA Urban 11 | Grand Challenge. 12 | 13 | ***AUTHORS 14 | 15 | ----------------------------------------- 16 | Jason Derenick, Ph.D. Candidate 17 | Computer Science and Engineering 18 | Lehigh University 19 | 19 Memorial Drive West 20 | Bethlehem, PA 18015 USA 21 | 22 | derenick(at)lehigh(dot)edu 23 | http://vader.cse.lehigh.edu/~derenick 24 | ----------------------------------------- 25 | 26 | ----------------------------------------- 27 | Thomas Miller 28 | Computer Science and Engineering 29 | Lehigh University 30 | 19 Memorial Drive West 31 | Bethlehem, PA 18015 USA 32 | 33 | thm204(at)lehigh(dot)edu 34 | http://www.lehigh.edu/~thm204 35 | ----------------------------------------- 36 | 37 | Jason and Thomas are advised by Professor John Spletzer 38 | in the Department of Computer Science and Engineering. 39 | 40 | ***ACKNOWLEDGEMENTS 41 | Please see the THANKS file :o) 42 | 43 | ------------------------------------------------------------------- -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package sicktoolbox 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.0.104 (2019-05-04) 6 | -------------------- 7 | 8 | 1.0.103 (2013-08-21) 9 | -------------------- 10 | * No more Willow Garage email. 11 | * Update CMakeLists.txt 12 | * Contributors: Chad Rockey 13 | 14 | 1.0.102 (2013-03-28) 15 | -------------------- 16 | * Update README 17 | * Catkinized and converted to CMake. 18 | * Initial package.xml and CmakeLists.txt 19 | * Applied power_delay.patch. 20 | * Applying unistd.patch 21 | * Applied stdlib_include.patch 22 | * Removing Matlab support. 23 | * Commiting configured files for cmake conversion. 24 | * Forgot an installation step for the INSTALL readme. 25 | * Updated install files, updated install directions. 26 | * Some minor changes 27 | * Cleaned up and began adjusting mex interface a bit for the lms1xx 28 | * Actually, the previous post should have been renamaing 1ms to lms2xx. Now I am adding lms1xx 29 | * Added directory for lms1xx in matlab examples directory 30 | * Added include for GCC4.3 31 | * A number of improvements, bug fixes. Also put together mex file for streaming values via the lms 1xx using the given C++ driver. Adjusted mex build scripts to build and include lms1xx. 32 | * deleting sicklms2xxlite 33 | * Changed name of sickld-1.0 to sickld 34 | * Moving mex lms dir to lms2xx 35 | * fixed some bugs and restructured the driver to provide more low-level control 36 | * Added range and reflectivity streaming; however, still can't figure out how to successfully transition between measurement modes without uninitializing. Lots of cleanup 37 | * Adjusted the lms1xx app and some minor cleanup 38 | * Added some range measurement acquisition to driver 39 | * Polished configuration functions 40 | * Adjusted lms1xx example file 41 | * committing cahnges to base and sick ld & lms2xx drivers 42 | * Fixed some bugs and setup configuration capability for the lms 1xx driver 43 | * Started adding _getSickStatus and began writing the message parsing code for the Sick LMS 1xx - currently the LMS 1xx will compile but is unstable 44 | * Setup connect/disconnect routines and buffer monitor for SickLMS1xx driver. Also, included an example for testing the code right now. 45 | * Started adding driver files for SickLMS1xx unit 46 | * Adjusted lms examples to using renamed sicklms2xx class. Also fixed corresponding makefiles and changed the prefix for each lms example to lms2xx (e.g. lms_config is now lms2xx_config) 47 | * Moved c++/examples/lms to c++/examples/lms2xx 48 | * Supplanted SickLMS with SickLMS2xx and sick_lms with sick_lms_2xx where appropriate. Currently compiles, but needs a sanity test 49 | * Created sicklms1xx driver directory and Makefile.am for each directory (currently, they are empty) 50 | * Updating configure.ac to use the new AX macros for lms1xx and lms2xx source directories 51 | * changes sicklms-1.0 to sicklms2xx and sicklmslite-1.0 to sicklms1xx 52 | * Adding macros for setting lms2xx and lms1xx source directories 53 | * Changed lms driver directory to lms2xx and created lms1xx drive directory in c++/drivers 54 | * Fixed a bug concerning dynamically allocating strings on the stack of an active exception. Thanks to Philipp Aumayr and Simon Opelt for their original patch, which motivated this fix. 55 | * Fixed quotation bug for newer versions of autoconf/aclocal. 56 | See http://www.gnu.org/software/autoconf/manual/html_node/Quoting-and-Parameters.html 57 | * Added stdlib.h to ld_config's main.cc and lms_config's main.cc 58 | * 59 | * 60 | * 61 | * 62 | * Added ld_lite directory 63 | * Addedd lms_lite directory 64 | * 65 | * 66 | * Added ld_lite folder for ld_lite driver 67 | * Added lms_lite directory for lms_lite driver 68 | * lmsmex.cc now supports up to four separate Sick LMS units. 69 | * Set lmsmex.cc to also return a vector of bearings for range and/or reflectivity values. The coordinate system corresponds to that defined in the Sick LMS 2xx manuals. Adjusted lms_cart and lms_stream examples to use this vector. 70 | * Removed SickConfig.hh from the driver headers and adjusted the installation to leave out SickConfig.hh and the utility headers as per Tully's suggestion. 71 | * Changed std::cerr to std::cout for printing 'cancel buffer monitor' string 72 | * Adjusted m-file comments for sickld and sicklms 73 | * Adjusted m-file comments 74 | * Fixing permissions 75 | * Still fixing file permissions 76 | * Adjusting file permissions 77 | * Took out std::cerr in bad checksum exception handler. 78 | * Took out std::cerr in bad checksum exception handler. 79 | * Monitor now clears buffer on bad checksum 80 | * Monitor now clears msg container on bad checksum 81 | * Fixed print-out in build_mex script 82 | * Took out packed attribute for structs in SickLMS.hh. 83 | * Removed try/catch in buffer monitor base 84 | * Adjusted the examples to exit more cleanly. 85 | * Fixed rm -r bug in build_mex and install_mex 86 | * Adjusted configure.ac 87 | * Fixing README 88 | * testing 89 | * Adjusted NEWS 90 | * Modified INSTALL 91 | * Fixing permissions on matlab/install_mex. 92 | * Fixing permissions with mex bash script. 93 | * Changed dates in mex installation bash scripts. 94 | * Removed doc directory from c++/drivers/ld. 95 | * Initial project import. 96 | * Contributors: Chad Rockey, Chris Mansley, Jason Derenick, Michael Sands, Tom Miller, chadrockey 97 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(sicktoolbox) 3 | 4 | find_package(catkin REQUIRED) 5 | 6 | ## System dependencies are found with CMake's conventions 7 | find_package(Threads REQUIRED) 8 | 9 | ################################### 10 | ## catkin specific configuration ## 11 | ################################### 12 | ## The catkin_package macro generates cmake config files for your package 13 | ## Declare things to be passed to dependent projects 14 | ## LIBRARIES: libraries you create in this project that dependent projects also need 15 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need 16 | ## DEPENDS: system dependencies of this project that dependent projects also need 17 | catkin_package( 18 | INCLUDE_DIRS include 19 | LIBRARIES SickLD SickLMS1xx SickLMS2xx 20 | CATKIN_DEPENDS 21 | DEPENDS Threads 22 | ) 23 | 24 | ########### 25 | ## Build ## 26 | ########### 27 | 28 | ## Specify additional locations of header files 29 | ## Your package locations should be listed before other locations 30 | include_directories(include ${catkin_INCLUDE_DIRS}) 31 | 32 | # Driver libraries 33 | add_library(SickLD c++/drivers/ld/sickld/SickLD.cc c++/drivers/ld/sickld/SickLDBufferMonitor.cc c++/drivers/ld/sickld/SickLDMessage.cc) 34 | target_link_libraries(SickLD ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) 35 | 36 | add_library(SickLMS1xx c++/drivers/lms1xx/sicklms1xx/SickLMS1xx.cc c++/drivers/lms1xx/sicklms1xx/SickLMS1xxBufferMonitor.cc c++/drivers/lms1xx/sicklms1xx/SickLMS1xxMessage.cc) 37 | target_link_libraries(SickLMS1xx ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) 38 | 39 | add_library(SickLMS2xx c++/drivers/lms2xx/sicklms2xx/SickLMS2xx.cc c++/drivers/lms2xx/sicklms2xx/SickLMS2xxBufferMonitor.cc c++/drivers/lms2xx/sicklms2xx/SickLMS2xxMessage.cc) 40 | target_link_libraries(SickLMS2xx ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT}) 41 | 42 | # Examples 43 | add_library(LDConfigLib c++/examples/ld/ld_config/src/ConfigFile.cpp) 44 | add_executable(ld_config c++/examples/ld/ld_config/src/main.cc) 45 | target_link_libraries(ld_config SickLD LDConfigLib ${catkin_LIBRARIES}) 46 | 47 | add_executable(ld_more_config c++/examples/ld/ld_more_config/src/main.cc) 48 | target_link_libraries(ld_more_config SickLD ${catkin_LIBRARIES}) 49 | 50 | add_executable(ld_multi_sector c++/examples/ld/ld_multi_sector/src/main.cc) 51 | target_link_libraries(ld_multi_sector SickLD ${catkin_LIBRARIES}) 52 | 53 | add_executable(ld_single_sector c++/examples/ld/ld_single_sector/src/main.cc) 54 | target_link_libraries(ld_single_sector SickLD ${catkin_LIBRARIES}) 55 | 56 | add_executable(lms1xx_simple_app c++/examples/lms1xx/lms1xx_simple_app/src/main.cc) 57 | target_link_libraries(lms1xx_simple_app SickLMS1xx ${catkin_LIBRARIES}) 58 | 59 | add_executable(lms2xx_config c++/examples/lms2xx/lms2xx_config/src/main.cc) 60 | target_link_libraries(lms2xx_config SickLMS2xx ${catkin_LIBRARIES}) 61 | 62 | add_executable(lms2xx_mean_values c++/examples/lms2xx/lms2xx_mean_values/src/main.cc) 63 | target_link_libraries(lms2xx_mean_values SickLMS2xx ${catkin_LIBRARIES}) 64 | 65 | add_executable(lms2xx_partial_scan c++/examples/lms2xx/lms2xx_partial_scan/src/main.cc) 66 | target_link_libraries(lms2xx_partial_scan SickLMS2xx ${catkin_LIBRARIES}) 67 | 68 | add_executable(lms2xx_real_time_indices c++/examples/lms2xx/lms2xx_real_time_indices/src/main.cc) 69 | target_link_libraries(lms2xx_real_time_indices SickLMS2xx ${catkin_LIBRARIES}) 70 | 71 | add_executable(lms2xx_set_variant c++/examples/lms2xx/lms2xx_set_variant/src/main.cc) 72 | target_link_libraries(lms2xx_set_variant SickLMS2xx ${catkin_LIBRARIES}) 73 | 74 | add_executable(lms2xx_simple_app c++/examples/lms2xx/lms2xx_simple_app/src/main.cc) 75 | target_link_libraries(lms2xx_simple_app SickLMS2xx ${catkin_LIBRARIES}) 76 | 77 | add_executable(lms2xx_stream_range_and_reflect c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/main.cc) 78 | target_link_libraries(lms2xx_stream_range_and_reflect SickLMS2xx ${catkin_LIBRARIES}) 79 | 80 | add_executable(lms2xx_subrange c++/examples/lms2xx/lms2xx_subrange/src/main.cc) 81 | target_link_libraries(lms2xx_subrange SickLMS2xx ${catkin_LIBRARIES}) 82 | 83 | ############# 84 | ## Install ## 85 | ############# 86 | 87 | # all install targets should use catkin DESTINATION variables 88 | # See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html 89 | 90 | ## Mark executable scripts (Python etc.) for installation 91 | ## in contrast to setup.py, you can choose the destination 92 | # install(PROGRAMS 93 | # scripts/my_python_script 94 | # DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 95 | # ) 96 | 97 | ## Mark executables and/or libraries for installation 98 | install(TARGETS SickLD SickLMS1xx SickLMS2xx LDConfigLib ld_config 99 | ld_more_config ld_multi_sector ld_single_sector lms1xx_simple_app 100 | lms2xx_config lms2xx_mean_values lms2xx_partial_scan lms2xx_real_time_indices 101 | lms2xx_set_variant lms2xx_simple_app lms2xx_stream_range_and_reflect lms2xx_subrange 102 | ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 103 | LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} 104 | RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 105 | ) 106 | 107 | ## TODO Move headers that aren't needed externally back into the source 108 | ## Mark cpp header files for installation 109 | install(DIRECTORY include/${PROJECT_NAME}/ 110 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 111 | FILES_MATCHING PATTERN "*.hh" 112 | PATTERN ".svn" EXCLUDE 113 | ) 114 | 115 | ## TODO There are readmes, etc, create install rules for these 116 | ## Mark other files for installation (e.g. launch and bag files, etc.) 117 | # install(FILES 118 | # # myfile1 119 | # # myfile2 120 | # DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 121 | # ) 122 | -------------------------------------------------------------------------------- /COPYING: -------------------------------------------------------------------------------- 1 | -------------------------------------------------------------------- 2 | -------------------------------------------------------------------- 3 | The Sick LIDAR Matlab/C++ Toolbox (BSD) License 4 | -------------------------------------------------------------------- 5 | 6 | The Sick LIDAR Matlab/C++ Toolbox 7 | Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 8 | All rights reserved. 9 | 10 | Redistribution and use in source and binary forms, with or without 11 | modification, are permitted provided that the following conditions 12 | are met: 13 | 14 | * Redistributions of source code must retain the above 15 | copyright notice, this list of conditions and the following 16 | disclaimer. 17 | * Redistributions in binary form must reproduce the above 18 | copyright notice, this list of conditions and the following 19 | disclaimer in the documentation and/or other materials 20 | provided with the distribution. 21 | * Neither the name(s) of the copyright holders nor the names 22 | of its contributors may be used to endorse or promote products 23 | derived from this software without specific prior written 24 | permission. 25 | 26 | THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 27 | "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 28 | LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 29 | FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 30 | COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 31 | INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 32 | BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 33 | LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 34 | CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 35 | LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 36 | ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 37 | POSSIBILITY OF SUCH DAMAGE. 38 | 39 | -------------------------------------------------------------------- 40 | -------------------------------------------------------------------------------- /README: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------- 2 | ------------------------------------------------------------------- 3 | The Sick LIDAR C++ Toolbox 4 | ------------------------------------------------------------------- 5 | 6 | This is the ROS fork of the Sick LIDAR C++ Toolbox. If you're 7 | looking for Matlab, please see the original at: 8 | http://sicktoolbox.sourceforge.net/ 9 | 10 | *** Quick Start 11 | To get up and running quickly with the toolbox, be sure to read 12 | the quick start guide in the manuals directory. Additionally, to 13 | enable 500Kbps communication via a USB-COMi-M be sure to see the 14 | RS-422 tutorial in said directory. 15 | 16 | *** Other Files 17 | Please see the following documents for additional info: 18 | 19 | COPYING - Information regarding the software license 20 | AUTHORS - Information about the authors 21 | THANKS - Acknowledgements 22 | 23 | ------------------------------------------------------------------- 24 | -------------------------------------------------------------------------------- /THANKS: -------------------------------------------------------------------------------- 1 | ------------------------------------------------------------------- 2 | ------------------------------------------------------------------- 3 | The Sick LIDAR Matlab/C++ Toolbox 4 | ------------------------------------------------------------------- 5 | 6 | The authors would like to express their sincere gratitude to the 7 | following members of the Ben Franklin Racing Team for their key 8 | insights and help in developing the toolbox: 9 | 10 | Aleksandr Kushleyev 11 | Tully Foote 12 | Alex Stewart 13 | Jon Bohren 14 | Brian Satterfield 15 | Professor Daniel Lee 16 | Professor John Spetzer 17 | 18 | Additionally, we'd like to thank Douglas Paul for his work on the 19 | initial Sick LMS 2xx serial driver. 20 | 21 | *** Additional Acknowledgments 22 | This software makes use of the following open-source 23 | packages for its package configuration: 24 | 25 | doxample - by Oren Ben-kiki 26 | http://www.ben-kiki.org/oren/doxample 27 | 28 | acx_pthread - by Steven G. Johnson 29 | http://autoconf-archive.cryp.to/acx_pthread.html 30 | 31 | ------------------------------------------------------------------- -------------------------------------------------------------------------------- /c++/drivers/ld/sickld/.deps/SickLD.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/ld/sickld/.deps/SickLDBufferMonitor.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/ld/sickld/.deps/SickLDMessage.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/ld/sickld/SickLDBufferMonitor.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLDBufferMonitor.cc 3 | * \brief Implements a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LD LIDAR. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /* Auto-generated header */ 18 | #include 19 | 20 | /* Implementation dependencies */ 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* Associate the namespace */ 30 | namespace SickToolbox { 31 | 32 | /** 33 | * \brief A standard constructor 34 | */ 35 | SickLDBufferMonitor::SickLDBufferMonitor( ) : SickBufferMonitor< SickLDBufferMonitor, SickLDMessage >(this) { } 36 | 37 | /** 38 | * \brief Acquires the next message from the SickLD byte stream 39 | * \param &sick_message The returned message object 40 | */ 41 | void SickLDBufferMonitor::GetNextMessageFromDataStream( SickLDMessage &sick_message ) throw( SickIOException ) { 42 | 43 | /* Flush the input buffer */ 44 | uint8_t byte_buffer; 45 | 46 | /* A buffer to hold the current byte out of the stream */ 47 | const uint8_t sick_response_header[4] = {0x02,'U','S','P'}; 48 | 49 | uint8_t checksum = 0; 50 | uint8_t message_buffer[SickLDMessage::MESSAGE_MAX_LENGTH] = {0}; 51 | uint32_t payload_length = 0; 52 | 53 | try { 54 | 55 | /* Search for the header in the byte stream */ 56 | for (unsigned int i = 0; i < sizeof(sick_response_header);) { 57 | 58 | /* Acquire the next byte from the stream */ 59 | _readBytes(&byte_buffer,1,DEFAULT_SICK_BYTE_TIMEOUT); 60 | 61 | /* Check if the current byte matches the expected header byte */ 62 | if (byte_buffer == sick_response_header[i]) { 63 | i++; 64 | } 65 | else { 66 | i = 0; 67 | } 68 | 69 | } 70 | 71 | /* Populate message buffer w/ response header */ 72 | memcpy(message_buffer,sick_response_header,4); 73 | 74 | /* Acquire the payload length! */ 75 | _readBytes(&message_buffer[4],4,DEFAULT_SICK_BYTE_TIMEOUT); 76 | 77 | /* Extract the payload size and adjust the byte order */ 78 | memcpy(&payload_length,&message_buffer[4],4); 79 | payload_length = sick_ld_to_host_byte_order(payload_length); 80 | 81 | /* Read the packet payload */ 82 | _readBytes(&message_buffer[8],payload_length,DEFAULT_SICK_BYTE_TIMEOUT); 83 | 84 | /* Read the checksum */ 85 | _readBytes(&checksum,1,DEFAULT_SICK_BYTE_TIMEOUT); 86 | 87 | /* Build the return message object based upon the received payload 88 | * and compute the associated checksum. 89 | * 90 | * NOTE: In constructing this message we ignore the header bytes 91 | * buffered since the BuildMessage routine will insert the 92 | * correct header automatically and compute the payload's 93 | * checksum for us. We could probably get away with using 94 | * just ParseMessage here and not computing the checksum as 95 | * we are using TCP. However, its safer this way. 96 | */ 97 | sick_message.BuildMessage(&message_buffer[SickLDMessage::MESSAGE_HEADER_LENGTH],payload_length); 98 | 99 | /* Verify the checksum is correct (this is probably unnecessary since we are using TCP/IP) */ 100 | if (sick_message.GetChecksum() != checksum) { 101 | throw SickBadChecksumException("SickLD::GetNextMessageFromDataStream: BAD CHECKSUM!!!"); 102 | } 103 | 104 | /* Success */ 105 | 106 | } 107 | 108 | catch(SickTimeoutException &sick_timeout) { /* This is ok! */ } 109 | 110 | /* Catch a bad checksum! */ 111 | catch(SickBadChecksumException &sick_checksum_exception) { 112 | sick_message.Clear(); // Clear the message container 113 | } 114 | 115 | /* Catch any serious IO buffer exceptions */ 116 | catch(SickIOException &sick_io_exception) { 117 | throw; 118 | } 119 | 120 | /* A sanity check */ 121 | catch (...) { 122 | throw; 123 | } 124 | 125 | } 126 | 127 | /** 128 | * \brief A standard destructor 129 | */ 130 | SickLDBufferMonitor::~SickLDBufferMonitor( ) { } 131 | 132 | } /* namespace SickToolbox */ 133 | -------------------------------------------------------------------------------- /c++/drivers/ld/sickld/SickLDMessage.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLDMessage.cc 3 | * \brief Implements the class SickLDMessage. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | /* Auto-generated header */ 17 | #include 18 | 19 | /* Implementation dependencies */ 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include // for byye-order conversions where necessary 26 | 27 | /* Associate the namespace */ 28 | namespace SickToolbox { 29 | 30 | /** 31 | * \brief A default constructor 32 | */ 33 | SickLDMessage::SickLDMessage( ) : 34 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() { 35 | 36 | /* Initialize the object */ 37 | Clear(); 38 | } 39 | 40 | /** 41 | * \brief Another constructor. 42 | * \param *payload_buffer The payload for the packet as an array of bytes (including the header) 43 | * \param payload_length The length of the payload array in bytes 44 | */ 45 | SickLDMessage::SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) : 46 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() { 47 | 48 | /* Build the message object (implicit initialization) */ 49 | BuildMessage(payload_buffer,payload_length); 50 | } 51 | 52 | /** 53 | * \brief Another constructor. 54 | * \param *message_buffer A well-formed message to be parsed into the class' fields 55 | */ 56 | SickLDMessage::SickLDMessage( const uint8_t * const message_buffer ) : 57 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() { 58 | 59 | /* Parse the message into the container (implicit initialization) */ 60 | ParseMessage(message_buffer); 61 | } 62 | 63 | /** 64 | * \brief Constructs a Sick LD message given parameter values 65 | * \param *payload_buffer An address of the first byte to be copied into the message's payload 66 | * \param payload_length The number of bytes to be copied into the message buffer 67 | */ 68 | void SickLDMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) { 69 | 70 | /* Call the parent method 71 | * NOTE: The parent method resets the object and assigns _message_length, _payload_length, 72 | * _populated and copies the payload into the message buffer at the correct position 73 | */ 74 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN > 75 | ::BuildMessage(payload_buffer,payload_length); 76 | 77 | /* 78 | * Set the message header! 79 | */ 80 | _message_buffer[0] = 0x02; // STX 81 | _message_buffer[1] = 'U'; // User 82 | _message_buffer[2] = 'S'; // Service 83 | _message_buffer[3] = 'P'; // Protocol 84 | 85 | /* Include the payload length in the header */ 86 | uint32_t payload_length_32 = host_to_sick_ld_byte_order((uint32_t)_payload_length); 87 | memcpy(&_message_buffer[4],&payload_length_32,4); 88 | 89 | /* 90 | * Set the message trailer (just a checksum)! 91 | */ 92 | _message_buffer[_message_length-1] = _computeXOR(&_message_buffer[8],(uint32_t)_payload_length); 93 | } 94 | 95 | /** 96 | * \brief Parses a sequence of bytes into a SickLDMessage object 97 | * \param *message_buffer A well-formed message to be parsed into the class' fields 98 | */ 99 | void SickLDMessage::ParseMessage( const uint8_t * const message_buffer ) { 100 | 101 | /* Call the parent method 102 | * NOTE: This method resets the object and assigns _populated as true 103 | */ 104 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN > 105 | ::ParseMessage(message_buffer); 106 | 107 | /* Extract the payload length */ 108 | uint32_t payload_length_32 = 0; 109 | memcpy(&payload_length_32,&message_buffer[4],4); 110 | _payload_length = (unsigned int)sick_ld_to_host_byte_order(payload_length_32); 111 | 112 | /* Compute the total message length */ 113 | _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length; 114 | 115 | /* Copy the given packet into the buffer */ 116 | memcpy(_message_buffer,message_buffer,_message_length); 117 | } 118 | 119 | /** 120 | * \brief Print the message contents. 121 | */ 122 | void SickLDMessage::Print( ) const { 123 | 124 | std::cout.setf(std::ios::hex,std::ios::basefield); 125 | std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl; 126 | std::cout << "Service code: " << (unsigned int) GetServiceCode() << std::endl; 127 | std::cout << "Service subcode: " << (unsigned int) GetServiceSubcode() << std::endl; 128 | std::cout << std::flush; 129 | 130 | /* Call parent's print function */ 131 | SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >::Print(); 132 | } 133 | 134 | /** 135 | * \brief Compute the message checksum (single-byte XOR). 136 | * \param data The address of the first data element in a sequence of bytes to be included in the sum 137 | * \param length The number of byte in the data sequence 138 | */ 139 | uint8_t SickLDMessage::_computeXOR( const uint8_t * const data, const uint32_t length ) { 140 | 141 | /* Compute the XOR by summing all of the bytes */ 142 | uint8_t checksum = 0; 143 | for (uint32_t i = 0; i < length; i++) { 144 | checksum ^= data[i]; // NOTE: this is equivalent to simply summing all of the bytes 145 | } 146 | 147 | /* done */ 148 | return checksum; 149 | } 150 | 151 | SickLDMessage::~SickLDMessage( ) { } 152 | 153 | } /* namespace SickToolbox */ 154 | -------------------------------------------------------------------------------- /c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xx.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xxBufferMonitor.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xxMessage.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms1xx/sicklms1xx/SickLMS1xxBufferMonitor.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xxBufferMonitor.cc 3 | * \brief Implements a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LMS 1xx LIDAR. 5 | * 6 | * Code by Jason C. Derenick and Christopher R. Mansley. 7 | * Contact jasonder(at)seas(dot)upenn(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /* Auto-generated header */ 18 | #include 19 | 20 | /* Implementation dependencies */ 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | /* Associate the namespace */ 30 | namespace SickToolbox { 31 | 32 | /** 33 | * \brief A standard constructor 34 | */ 35 | SickLMS1xxBufferMonitor::SickLMS1xxBufferMonitor( ) : SickBufferMonitor< SickLMS1xxBufferMonitor, SickLMS1xxMessage >(this) { } 36 | 37 | /** 38 | * \brief Acquires the next message from the SickLMS1xx byte stream 39 | * \param &sick_message The returned message object 40 | */ 41 | void SickLMS1xxBufferMonitor::GetNextMessageFromDataStream( SickLMS1xxMessage &sick_message ) throw( SickIOException ) { 42 | 43 | /* Flush the input buffer */ 44 | uint8_t byte_buffer = 0; 45 | uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0}; 46 | 47 | try { 48 | 49 | /* Flush the TCP receive buffer */ 50 | _flushTCPRecvBuffer(); 51 | 52 | /* Search for STX in the byte stream */ 53 | do { 54 | 55 | /* Grab the next byte from the stream */ 56 | _readBytes(&byte_buffer,1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT); 57 | 58 | } 59 | while (byte_buffer != 0x02); 60 | 61 | /* Ok, now acquire the payload! (until ETX) */ 62 | int payload_length = 0; 63 | do { 64 | 65 | payload_length++; 66 | _readBytes(&payload_buffer[payload_length-1],1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT); 67 | 68 | } 69 | while (payload_buffer[payload_length-1] != 0x03); 70 | payload_length--; 71 | 72 | /* Build the return message object based upon the received payload 73 | * NOTE: In constructing this message we ignore the header bytes 74 | * buffered since the BuildMessage routine will insert the 75 | * correct header automatically and verify the message size 76 | */ 77 | sick_message.BuildMessage(payload_buffer,payload_length); 78 | 79 | /* Success */ 80 | 81 | } 82 | 83 | catch(SickTimeoutException &sick_timeout) { /* This is ok! */ } 84 | 85 | /* Catch any serious IO buffer exceptions */ 86 | catch(SickIOException &sick_io_exception) { 87 | throw; 88 | } 89 | 90 | /* A sanity check */ 91 | catch (...) { 92 | throw; 93 | } 94 | 95 | } 96 | 97 | /** 98 | * \brief Flushes TCP receive buffer contents 99 | */ 100 | void SickLMS1xxBufferMonitor::_flushTCPRecvBuffer( ) const throw (SickIOException) { 101 | 102 | char null_byte; 103 | int num_bytes_waiting = 0; 104 | 105 | /* Acquire number of awaiting bytes */ 106 | if (ioctl(_sick_fd,FIONREAD,&num_bytes_waiting)) { 107 | throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!"); 108 | } 109 | 110 | /* Flush awaiting bytes */ 111 | for (int i = 0; i < num_bytes_waiting; i++) { 112 | 113 | /* Capture a single byte from the stream! */ 114 | if (read(_sick_fd,&null_byte,1) != 1) { 115 | throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!"); 116 | } 117 | 118 | } 119 | 120 | } 121 | 122 | /** 123 | * \brief A standard destructor 124 | */ 125 | SickLMS1xxBufferMonitor::~SickLMS1xxBufferMonitor( ) { } 126 | 127 | } /* namespace SickToolbox */ 128 | -------------------------------------------------------------------------------- /c++/drivers/lms1xx/sicklms1xx/SickLMS1xxMessage.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xxMessage.cc 3 | * \brief Implements the class SickLMS1xxMessage. 4 | * 5 | * Code by Jason C. Derenick and Christopher R. Mansley. 6 | * Contact jasonder(at)seas(dot)upenn(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | /* Auto-generated header */ 17 | #include 18 | 19 | /* Implementation dependencies */ 20 | #include 21 | #include 22 | #include 23 | 24 | #include 25 | #include // for byye-order conversions where necessary 26 | 27 | /* Associate the namespace */ 28 | namespace SickToolbox { 29 | 30 | /** 31 | * \brief A default constructor 32 | */ 33 | SickLMS1xxMessage::SickLMS1xxMessage( ) : 34 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(), 35 | _command_type(""), 36 | _command("") 37 | { 38 | 39 | /* Initialize the object */ 40 | Clear(); 41 | 42 | } 43 | 44 | /** 45 | * \brief Another constructor. 46 | * \param *payload_buffer The payload for the packet as an array of bytes (including the header) 47 | * \param payload_length The length of the payload array in bytes 48 | */ 49 | SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) : 50 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(), 51 | _command_type("Unknown"), 52 | _command("Unknown") 53 | { 54 | 55 | /* Build the message object (implicit initialization) */ 56 | BuildMessage(payload_buffer,payload_length); 57 | 58 | } 59 | 60 | /** 61 | * \brief Another constructor. 62 | * \param *message_buffer A well-formed message to be parsed into the class' fields 63 | */ 64 | SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const message_buffer ) : 65 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(), 66 | _command_type("Unknown"), 67 | _command("Unknown") 68 | { 69 | 70 | /* Parse the message into the container (implicit initialization) */ 71 | ParseMessage(message_buffer); 72 | 73 | } 74 | 75 | /** 76 | * \brief Constructs a well-formed Sick LMS 1xx message 77 | * \param *payload_buffer An address of the first byte to be copied into the message's payload 78 | * \param payload_length The number of bytes to be copied into the message buffer 79 | */ 80 | void SickLMS1xxMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) { 81 | 82 | /* Call the parent method 83 | * NOTE: The parent method resets the object and assigns _message_length, _payload_length, 84 | * _populated and copies the payload into the message buffer at the correct position 85 | */ 86 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN > 87 | ::BuildMessage(payload_buffer,payload_length); 88 | 89 | /* 90 | * Set the message header! 91 | */ 92 | _message_buffer[0] = 0x02; // STX 93 | 94 | /* 95 | * Set the message trailer! 96 | */ 97 | _message_buffer[_message_length-1] = 0x03; // ETX 98 | 99 | /* Grab the (3-byte) command type */ 100 | char command_type[4] = {0}; 101 | for (int i = 0; i < 3; i++) { 102 | command_type[i] = _message_buffer[i+1]; 103 | } 104 | command_type[4] = '\0'; 105 | _command_type = command_type; 106 | 107 | /* Grab the command (max length is 14 bytes) */ 108 | char command[15] = {0}; 109 | int i = 0; 110 | for (; (i < 14) && (_message_buffer[5+i] != 0x20); i++) { 111 | command[i] = _message_buffer[5+i]; 112 | } 113 | command[i] = '\0'; 114 | _command = command; 115 | 116 | } 117 | 118 | /** 119 | * \brief Parses a sequence of bytes into a SickLMS1xxMessage object 120 | * \param *message_buffer A well-formed message to be parsed into the class' fields 121 | */ 122 | void SickLMS1xxMessage::ParseMessage( const uint8_t * const message_buffer ) throw (SickIOException) { 123 | 124 | /* Call the parent method 125 | * NOTE: This method resets the object and assigns _populated as true 126 | */ 127 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN > 128 | ::ParseMessage(message_buffer); 129 | 130 | /* Compute the message length */ 131 | int i = 1; 132 | const char * token = NULL; 133 | while (message_buffer[i-1] != 0x03) { 134 | 135 | if (i == 1) { 136 | 137 | /* Grab the command type */ 138 | if ((token = strtok((char *)&_message_buffer[1]," ")) == NULL) { 139 | throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!"); 140 | } 141 | 142 | _command_type = token; 143 | 144 | /* Grab the Command Code */ 145 | if ((token = strtok(NULL," ")) == NULL) { 146 | throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!"); 147 | } 148 | 149 | _command = token; 150 | 151 | } 152 | 153 | i++; // Update message length 154 | 155 | /* A sanity check */ 156 | if (i > SickLMS1xxMessage::MESSAGE_MAX_LENGTH) { 157 | throw SickIOException("SickLMS1xxMessage::ParseMessage: Message Exceeds Max Message Length!"); 158 | } 159 | 160 | } 161 | 162 | /* Compute the total message length */ 163 | _payload_length = _message_length - MESSAGE_HEADER_LENGTH - MESSAGE_TRAILER_LENGTH; 164 | 165 | /* Copy the given packet into the buffer */ 166 | memcpy(_message_buffer,message_buffer,_message_length); 167 | 168 | } 169 | 170 | /** 171 | * \brief Reset all internal fields and buffers associated with the object. 172 | */ 173 | void SickLMS1xxMessage::Clear( ) { 174 | 175 | /* Call the parent method and clear out class' protected members */ 176 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Clear(); 177 | 178 | /* Reset the class' additional fields */ 179 | _command_type = "Unknown"; 180 | _command = "Unknown"; 181 | 182 | } 183 | 184 | /** 185 | * \brief Print the message contents. 186 | */ 187 | void SickLMS1xxMessage::Print( ) const { 188 | 189 | //std::cout.setf(std::ios::hex,std::ios::basefield); 190 | std::cout << "Command Type: " << GetCommandType() << std::endl; 191 | std::cout << "Command Code: " << GetCommand() << std::endl; 192 | std::cout << std::flush; 193 | 194 | /* Call parent's print function */ 195 | SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Print(); 196 | 197 | } 198 | 199 | SickLMS1xxMessage::~SickLMS1xxMessage( ) { } 200 | 201 | } /* namespace SickToolbox */ 202 | -------------------------------------------------------------------------------- /c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xx.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xxBufferMonitor.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xxMessage.Plo: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/drivers/lms2xx/sicklms2xx/SickLMS2xxBufferMonitor.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS2xxBufferMonitor.cc 3 | * \brief Implements a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LMS 2xx LIDAR. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /* Auto-generated header */ 18 | #include 19 | 20 | /* Implementation dependencies */ 21 | #include 22 | #include 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | /* Associate the namespace */ 31 | namespace SickToolbox { 32 | 33 | /** 34 | * \brief A standard constructor 35 | */ 36 | SickLMS2xxBufferMonitor::SickLMS2xxBufferMonitor( ) : SickBufferMonitor(this) { } 37 | 38 | /** 39 | * \brief Acquires the next message from the SickLMS2xx byte stream 40 | * \param &sick_message The returned message object 41 | */ 42 | void SickLMS2xxBufferMonitor::GetNextMessageFromDataStream( SickLMS2xxMessage &sick_message ) throw( SickIOException ) { 43 | 44 | uint8_t search_buffer[2] = {0}; 45 | uint8_t payload_length_buffer[2] = {0}; 46 | uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0}; 47 | uint8_t checksum_buffer[2] = {0}; 48 | uint16_t payload_length, checksum; 49 | 50 | try { 51 | 52 | /* Drain the I/O buffers! */ 53 | if (tcdrain(_sick_fd) != 0) { 54 | throw SickIOException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: tcdrain failed!"); 55 | } 56 | 57 | /* Read until we get a valid message header */ 58 | unsigned int bytes_searched = 0; 59 | while(search_buffer[0] != 0x02 || search_buffer[1] != DEFAULT_SICK_LMS_2XX_HOST_ADDRESS) { 60 | 61 | /* Slide the search window */ 62 | search_buffer[0] = search_buffer[1]; 63 | 64 | /* Attempt to read in another byte */ 65 | _readBytes(&search_buffer[1],1,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT); 66 | 67 | /* Header should be no more than max message length + header length bytes away */ 68 | if (bytes_searched > SickLMS2xxMessage::MESSAGE_MAX_LENGTH + SickLMS2xxMessage::MESSAGE_HEADER_LENGTH) { 69 | throw SickTimeoutException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: header timeout!"); 70 | } 71 | 72 | /* Increment the number of bytes searched */ 73 | bytes_searched++; 74 | 75 | } 76 | 77 | /* Read until we receive the payload length or we timeout */ 78 | _readBytes(payload_length_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT); 79 | 80 | /* Extract the payload length */ 81 | memcpy(&payload_length,payload_length_buffer,2); 82 | payload_length = sick_lms_2xx_to_host_byte_order(payload_length); 83 | 84 | /* Make sure the payload length is legitimate, otherwise disregard */ 85 | if (payload_length <= SickLMS2xxMessage::MESSAGE_MAX_LENGTH) { 86 | 87 | /* Read until we receive the payload or we timeout */ 88 | _readBytes(payload_buffer,payload_length,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT); 89 | 90 | /* Read until we receive the checksum or we timeout */ 91 | _readBytes(checksum_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT); 92 | 93 | /* Copy into uint16_t so it can be used */ 94 | memcpy(&checksum,checksum_buffer,2); 95 | checksum = sick_lms_2xx_to_host_byte_order(checksum); 96 | 97 | /* Build a frame and compute the crc */ 98 | sick_message.BuildMessage(DEFAULT_SICK_LMS_2XX_HOST_ADDRESS,payload_buffer,payload_length); 99 | 100 | /* See if the checksums match */ 101 | if(sick_message.GetChecksum() != checksum) { 102 | throw SickBadChecksumException("SickLMS2xx::GetNextMessageFromDataStream: CRC16 failed!"); 103 | } 104 | 105 | } 106 | 107 | } 108 | 109 | catch(SickTimeoutException &sick_timeout_exception) { /* This is ok! */ } 110 | 111 | /* Handle a bad checksum! */ 112 | catch(SickBadChecksumException &sick_checksum_exception) { 113 | sick_message.Clear(); // Clear the message container 114 | } 115 | 116 | /* Handle any serious IO exceptions */ 117 | catch(SickIOException &sick_io_exception) { 118 | throw; 119 | } 120 | 121 | /* A sanity check */ 122 | catch (...) { 123 | throw; 124 | } 125 | 126 | } 127 | 128 | /** 129 | * \brief A standard destructor 130 | */ 131 | SickLMS2xxBufferMonitor::~SickLMS2xxBufferMonitor( ) { } 132 | 133 | } /* namespace SickToolbox */ 134 | -------------------------------------------------------------------------------- /c++/drivers/lms2xx/sicklms2xx/SickLMS2xxMessage.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS2xxMessage.cc 3 | * \brief Implementation of class SickFrame. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | /* Auto-generated header */ 17 | #include 18 | 19 | /* Implementation dependencies */ 20 | #include 21 | #include 22 | 23 | #include 24 | #include 25 | 26 | /* Associate the namespace */ 27 | namespace SickToolbox { 28 | 29 | /*! 30 | * \brief A default constructor 31 | */ 32 | SickLMS2xxMessage::SickLMS2xxMessage( ) { 33 | 34 | /* Initialize the object */ 35 | Clear(); 36 | } 37 | 38 | /*! 39 | * \brief A constructor for building a message from the given parameter values 40 | * \param dest_address The source address of the message 41 | * \param payload_buffer The payload of the message as an array of bytes (including the command code) 42 | * \param payload_length The length of the payload array in bytes 43 | */ 44 | SickLMS2xxMessage::SickLMS2xxMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length ) : 45 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >() { 46 | 47 | /* Build the message */ 48 | BuildMessage(dest_address,payload_buffer,payload_length); 49 | } 50 | 51 | /*! 52 | * \brief A constructor for parsing a byte sequence into a message object 53 | * \param message_buffer A well-formed message to be parsed into the class' fields 54 | */ 55 | SickLMS2xxMessage::SickLMS2xxMessage( uint8_t * const message_buffer ) : 56 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >() { 57 | 58 | /* Parse the byte sequence into a message object */ 59 | ParseMessage(message_buffer); 60 | } 61 | 62 | /*! 63 | * \brief Consructs a message object from given parameter values 64 | * \param dest_address The destination address of the frame 65 | * \param *payload_buffer The payload for the frame as an array of bytes (including the command code) 66 | * \param payload_length The length of the payload array in bytes 67 | */ 68 | void SickLMS2xxMessage::BuildMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, 69 | const unsigned int payload_length ) { 70 | 71 | /* Call the parent method! 72 | * NOTE: The parent method resets the object and assigns _message_length, _payload_length, 73 | * _populated and copies the payload into the message buffer at the correct position 74 | */ 75 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN > 76 | ::BuildMessage(payload_buffer,payload_length); 77 | 78 | /* 79 | * Set the message header! 80 | */ 81 | _message_buffer[0] = 0x02; // Start of transmission (stx) 82 | _message_buffer[1] = dest_address; // Sick LMS address 83 | 84 | /* Include the payload length in the header */ 85 | uint16_t payload_length_16 = host_to_sick_lms_2xx_byte_order((uint16_t)_payload_length); 86 | memcpy(&_message_buffer[2],&payload_length_16,2); 87 | 88 | /* 89 | * Set the message trailer! 90 | */ 91 | _checksum = host_to_sick_lms_2xx_byte_order(_computeCRC(_message_buffer,_payload_length+4)); 92 | memcpy(&_message_buffer[_payload_length+4],&_checksum,2); 93 | 94 | } 95 | 96 | /*! 97 | * \brief Parses a sequence of bytes into a well-formed message 98 | * \param *message_buffer The buffer containing the source message 99 | */ 100 | void SickLMS2xxMessage::ParseMessage( const uint8_t * const message_buffer ) { 101 | 102 | /* Call the parent method! 103 | * NOTE: This method resets the object and assigns _populated as true 104 | */ 105 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN > 106 | ::ParseMessage(message_buffer); 107 | 108 | /* Extract the payload length */ 109 | uint16_t payload_length_16 = 0; 110 | memcpy(&payload_length_16,&message_buffer[2],2); 111 | _payload_length = (unsigned int)sick_lms_2xx_to_host_byte_order(payload_length_16); 112 | 113 | /* Compute the total message length */ 114 | _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length; 115 | 116 | /* Copy the give message into the buffer */ 117 | memcpy(_message_buffer, message_buffer,_message_length); 118 | 119 | /* Extract the checksum from the frame */ 120 | memcpy(&_checksum,&_message_buffer[_payload_length+MESSAGE_HEADER_LENGTH],2); 121 | _checksum = sick_lms_2xx_to_host_byte_order(_checksum); 122 | 123 | } 124 | 125 | /*! 126 | * \brief Reset all internal fields and buffers associated with the object. 127 | */ 128 | void SickLMS2xxMessage::Clear( ) { 129 | 130 | /* Call the parent method and clear out class' protected members */ 131 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Clear(); 132 | 133 | /* Reset the class' additional fields */ 134 | _checksum = 0; 135 | 136 | } 137 | 138 | /*! 139 | * \brief Print the message contents. 140 | */ 141 | void SickLMS2xxMessage::Print( ) const { 142 | 143 | std::cout.setf(std::ios::hex,std::ios::basefield); 144 | std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl; 145 | std::cout << "Dest. Addr.: " << (unsigned int) GetDestAddress() << std::endl; 146 | std::cout << "Command Code: " << (unsigned int) GetCommandCode() << std::endl; 147 | std::cout << std::flush; 148 | 149 | /* Call parent's print function */ 150 | SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Print(); 151 | } 152 | 153 | /*! 154 | * \brief Computes the CRC16 of the given data buffer 155 | * \param data An array of bytes whose checksum to compute 156 | * \param len The length of the data array 157 | * \return CRC16 computed over given data buffer 158 | */ 159 | uint16_t SickLMS2xxMessage::_computeCRC( uint8_t * data, unsigned int data_length ) const { 160 | 161 | uint16_t uCrc16; 162 | uint8_t abData[2]; 163 | uCrc16 = abData[0] = 0; 164 | while (data_length-- ) { 165 | abData[1] = abData[0]; 166 | abData[0] = *data++; 167 | if(uCrc16 & 0x8000) { 168 | uCrc16 = (uCrc16 & 0x7fff) << 1; 169 | uCrc16 ^= CRC16_GEN_POL; 170 | } 171 | else { 172 | uCrc16 <<= 1; 173 | } 174 | uCrc16 ^= MKSHORT(abData[0],abData[1]); 175 | } 176 | return uCrc16; 177 | } 178 | 179 | SickLMS2xxMessage::~SickLMS2xxMessage( ) { } 180 | 181 | } /* namespace SickToolbox */ 182 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/conf/sickld.conf: -------------------------------------------------------------------------------- 1 | ############################################################### 2 | # 3 | # The Sick LIDAR Matlab/C++ Toolbox 4 | # 5 | # File: sickld.conf 6 | # Auth: Jason Derenick and Thomas Miller at Lehigh University 7 | # Cont: derenick(at)lehigh(dot)edu 8 | # Date: 20 July 2007 9 | # 10 | # Desc: Sample config file for ld_config utility. 11 | # 12 | ############################################################### 13 | 14 | # Define the Sick LD motor speed (Hz) 15 | SICK_LD_MOTOR_SPEED = 10 16 | 17 | # Define the Sick LD scan res. (angle step) in degrees 18 | SICK_LD_SCAN_RESOLUTION = 0.5 19 | 20 | # Define the active scan areas for the device 21 | SICK_LD_SCAN_AREAS = [90 270] [315 45] 22 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/doxygen-html-header: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/src/.deps/ConfigFile.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/src/ConfigFile.cpp: -------------------------------------------------------------------------------- 1 | // ConfigFile.cpp 2 | 3 | #include "ConfigFile.h" 4 | 5 | using std::string; 6 | 7 | ConfigFile::ConfigFile( string filename, string delimiter, 8 | string comment, string sentry ) 9 | : myDelimiter(delimiter), myComment(comment), mySentry(sentry) 10 | { 11 | // Construct a ConfigFile, getting keys and values from given file 12 | 13 | std::ifstream in( filename.c_str() ); 14 | 15 | if( !in ) throw file_not_found( filename ); 16 | 17 | in >> (*this); 18 | } 19 | 20 | 21 | ConfigFile::ConfigFile() 22 | : myDelimiter( string(1,'=') ), myComment( string(1,'#') ) 23 | { 24 | // Construct a ConfigFile without a file; empty 25 | } 26 | 27 | 28 | void ConfigFile::remove( const string& key ) 29 | { 30 | // Remove key and its value 31 | myContents.erase( myContents.find( key ) ); 32 | return; 33 | } 34 | 35 | 36 | bool ConfigFile::keyExists( const string& key ) const 37 | { 38 | // Indicate whether key is found 39 | mapci p = myContents.find( key ); 40 | return ( p != myContents.end() ); 41 | } 42 | 43 | 44 | /* static */ 45 | void ConfigFile::trim( string& s ) 46 | { 47 | // Remove leading and trailing whitespace 48 | static const char whitespace[] = " \n\t\v\r\f"; 49 | s.erase( 0, s.find_first_not_of(whitespace) ); 50 | s.erase( s.find_last_not_of(whitespace) + 1U ); 51 | } 52 | 53 | 54 | std::ostream& operator<<( std::ostream& os, const ConfigFile& cf ) 55 | { 56 | // Save a ConfigFile to os 57 | for( ConfigFile::mapci p = cf.myContents.begin(); 58 | p != cf.myContents.end(); 59 | ++p ) 60 | { 61 | os << p->first << " " << cf.myDelimiter << " "; 62 | os << p->second << std::endl; 63 | } 64 | return os; 65 | } 66 | 67 | 68 | std::istream& operator>>( std::istream& is, ConfigFile& cf ) 69 | { 70 | // Load a ConfigFile from is 71 | // Read in keys and values, keeping internal whitespace 72 | typedef string::size_type pos; 73 | const string& delim = cf.myDelimiter; // separator 74 | const string& comm = cf.myComment; // comment 75 | const string& sentry = cf.mySentry; // end of file sentry 76 | const pos skip = delim.length(); // length of separator 77 | 78 | string nextline = ""; // might need to read ahead to see where value ends 79 | 80 | while( is || nextline.length() > 0 ) 81 | { 82 | // Read an entire line at a time 83 | string line; 84 | if( nextline.length() > 0 ) 85 | { 86 | line = nextline; // we read ahead; use it now 87 | nextline = ""; 88 | } 89 | else 90 | { 91 | std::getline( is, line ); 92 | } 93 | 94 | // Ignore comments 95 | line = line.substr( 0, line.find(comm) ); 96 | 97 | // Check for end of file sentry 98 | if( sentry != "" && line.find(sentry) != string::npos ) return is; 99 | 100 | // Parse the line if it contains a delimiter 101 | pos delimPos = line.find( delim ); 102 | if( delimPos < string::npos ) 103 | { 104 | // Extract the key 105 | string key = line.substr( 0, delimPos ); 106 | line.replace( 0, delimPos+skip, "" ); 107 | 108 | // See if value continues on the next line 109 | // Stop at blank line, next line with a key, end of stream, 110 | // or end of file sentry 111 | bool terminate = false; 112 | while( !terminate && is ) 113 | { 114 | std::getline( is, nextline ); 115 | terminate = true; 116 | 117 | string nlcopy = nextline; 118 | ConfigFile::trim(nlcopy); 119 | if( nlcopy == "" ) continue; 120 | 121 | nextline = nextline.substr( 0, nextline.find(comm) ); 122 | if( nextline.find(delim) != string::npos ) 123 | continue; 124 | if( sentry != "" && nextline.find(sentry) != string::npos ) 125 | continue; 126 | 127 | nlcopy = nextline; 128 | ConfigFile::trim(nlcopy); 129 | if( nlcopy != "" ) line += "\n"; 130 | line += nextline; 131 | terminate = false; 132 | } 133 | 134 | // Store key and value 135 | ConfigFile::trim(key); 136 | ConfigFile::trim(line); 137 | cf.myContents[key] = line; // overwrites if key is repeated 138 | } 139 | } 140 | 141 | return is; 142 | } 143 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/src/ConfigFile.h: -------------------------------------------------------------------------------- 1 | // ConfigFile.h 2 | // Class for reading named values from configuration files 3 | // Richard J. Wagner v2.1 24 May 2004 wagnerr@umich.edu 4 | 5 | // Copyright (c) 2004 Richard J. Wagner 6 | // 7 | // Permission is hereby granted, free of charge, to any person obtaining a copy 8 | // of this software and associated documentation files (the "Software"), to 9 | // deal in the Software without restriction, including without limitation the 10 | // rights to use, copy, modify, merge, publish, distribute, sublicense, and/or 11 | // sell copies of the Software, and to permit persons to whom the Software is 12 | // furnished to do so, subject to the following conditions: 13 | // 14 | // The above copyright notice and this permission notice shall be included in 15 | // all copies or substantial portions of the Software. 16 | // 17 | // THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR 18 | // IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY, 19 | // FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE 20 | // AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER 21 | // LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING 22 | // FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS 23 | // IN THE SOFTWARE. 24 | 25 | // Typical usage 26 | // ------------- 27 | // 28 | // Given a configuration file "settings.inp": 29 | // atoms = 25 30 | // length = 8.0 # nanometers 31 | // name = Reece Surcher 32 | // 33 | // Named values are read in various ways, with or without default values: 34 | // ConfigFile config( "settings.inp" ); 35 | // int atoms = config.read( "atoms" ); 36 | // double length = config.read( "length", 10.0 ); 37 | // string author, title; 38 | // config.readInto( author, "name" ); 39 | // config.readInto( title, "title", string("Untitled") ); 40 | // 41 | // See file example.cpp for more examples. 42 | 43 | #ifndef CONFIGFILE_H 44 | #define CONFIGFILE_H 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | using std::string; 53 | 54 | class ConfigFile { 55 | // Data 56 | protected: 57 | string myDelimiter; // separator between key and value 58 | string myComment; // separator between value and comments 59 | string mySentry; // optional string to signal end of file 60 | std::map myContents; // extracted keys and values 61 | 62 | typedef std::map::iterator mapi; 63 | typedef std::map::const_iterator mapci; 64 | 65 | // Methods 66 | public: 67 | ConfigFile( string filename, 68 | string delimiter = "=", 69 | string comment = "#", 70 | string sentry = "EndConfigFile" ); 71 | ConfigFile(); 72 | 73 | // Search for key and read value or optional default value 74 | template T read( const string& key ) const; // call as read 75 | template T read( const string& key, const T& value ) const; 76 | template bool readInto( T& var, const string& key ) const; 77 | template 78 | bool readInto( T& var, const string& key, const T& value ) const; 79 | 80 | // Modify keys and values 81 | template void add( string key, const T& value ); 82 | void remove( const string& key ); 83 | 84 | // Check whether key exists in configuration 85 | bool keyExists( const string& key ) const; 86 | 87 | // Check or change configuration syntax 88 | string getDelimiter() const { return myDelimiter; } 89 | string getComment() const { return myComment; } 90 | string getSentry() const { return mySentry; } 91 | string setDelimiter( const string& s ) 92 | { string old = myDelimiter; myDelimiter = s; return old; } 93 | string setComment( const string& s ) 94 | { string old = myComment; myComment = s; return old; } 95 | 96 | // Write or read configuration 97 | friend std::ostream& operator<<( std::ostream& os, const ConfigFile& cf ); 98 | friend std::istream& operator>>( std::istream& is, ConfigFile& cf ); 99 | 100 | protected: 101 | template static string T_as_string( const T& t ); 102 | template static T string_as_T( const string& s ); 103 | static void trim( string& s ); 104 | 105 | 106 | // Exception types 107 | public: 108 | struct file_not_found { 109 | string filename; 110 | file_not_found( const string& filename_ = string() ) 111 | : filename(filename_) {} }; 112 | struct key_not_found { // thrown only by T read(key) variant of read() 113 | string key; 114 | key_not_found( const string& key_ = string() ) 115 | : key(key_) {} }; 116 | }; 117 | 118 | 119 | /* static */ 120 | template 121 | string ConfigFile::T_as_string( const T& t ) 122 | { 123 | // Convert from a T to a string 124 | // Type T must support << operator 125 | std::ostringstream ost; 126 | ost << t; 127 | return ost.str(); 128 | } 129 | 130 | 131 | /* static */ 132 | template 133 | T ConfigFile::string_as_T( const string& s ) 134 | { 135 | // Convert from a string to a T 136 | // Type T must support >> operator 137 | T t; 138 | std::istringstream ist(s); 139 | ist >> t; 140 | return t; 141 | } 142 | 143 | 144 | /* static */ 145 | template<> 146 | inline string ConfigFile::string_as_T( const string& s ) 147 | { 148 | // Convert from a string to a string 149 | // In other words, do nothing 150 | return s; 151 | } 152 | 153 | 154 | /* static */ 155 | template<> 156 | inline bool ConfigFile::string_as_T( const string& s ) 157 | { 158 | // Convert from a string to a bool 159 | // Interpret "false", "F", "no", "n", "0" as false 160 | // Interpret "true", "T", "yes", "y", "1", "-1", or anything else as true 161 | bool b = true; 162 | string sup = s; 163 | for( string::iterator p = sup.begin(); p != sup.end(); ++p ) 164 | *p = toupper(*p); // make string all caps 165 | if( sup==string("FALSE") || sup==string("F") || 166 | sup==string("NO") || sup==string("N") || 167 | sup==string("0") || sup==string("NONE") ) 168 | b = false; 169 | return b; 170 | } 171 | 172 | 173 | template 174 | T ConfigFile::read( const string& key ) const 175 | { 176 | // Read the value corresponding to key 177 | mapci p = myContents.find(key); 178 | if( p == myContents.end() ) throw key_not_found(key); 179 | return string_as_T( p->second ); 180 | } 181 | 182 | 183 | template 184 | T ConfigFile::read( const string& key, const T& value ) const 185 | { 186 | // Return the value corresponding to key or given default value 187 | // if key is not found 188 | mapci p = myContents.find(key); 189 | if( p == myContents.end() ) return value; 190 | return string_as_T( p->second ); 191 | } 192 | 193 | 194 | template 195 | bool ConfigFile::readInto( T& var, const string& key ) const 196 | { 197 | // Get the value corresponding to key and store in var 198 | // Return true if key is found 199 | // Otherwise leave var untouched 200 | mapci p = myContents.find(key); 201 | bool found = ( p != myContents.end() ); 202 | if( found ) var = string_as_T( p->second ); 203 | return found; 204 | } 205 | 206 | 207 | template 208 | bool ConfigFile::readInto( T& var, const string& key, const T& value ) const 209 | { 210 | // Get the value corresponding to key and store in var 211 | // Return true if key is found 212 | // Otherwise set var to given default 213 | mapci p = myContents.find(key); 214 | bool found = ( p != myContents.end() ); 215 | if( found ) 216 | var = string_as_T( p->second ); 217 | else 218 | var = value; 219 | return found; 220 | } 221 | 222 | 223 | template 224 | void ConfigFile::add( string key, const T& value ) 225 | { 226 | // Add a key with given value 227 | string v = T_as_string( value ); 228 | trim(key); 229 | trim(v); 230 | myContents[key] = v; 231 | return; 232 | } 233 | 234 | #endif // CONFIGFILE_H 235 | 236 | // Release notes: 237 | // v1.0 21 May 1999 238 | // + First release 239 | // + Template read() access only through non-member readConfigFile() 240 | // + ConfigurationFileBool is only built-in helper class 241 | // 242 | // v2.0 3 May 2002 243 | // + Shortened name from ConfigurationFile to ConfigFile 244 | // + Implemented template member functions 245 | // + Changed default comment separator from % to # 246 | // + Enabled reading of multiple-line values 247 | // 248 | // v2.1 24 May 2004 249 | // + Made template specializations inline to avoid compiler-dependent linkage 250 | // + Allowed comments within multiple-line values 251 | // + Enabled blank line termination for multiple-line values 252 | // + Added optional sentry to detect end of configuration file 253 | // + Rewrote messy trimWhitespace() function as elegant trim() 254 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_config/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple configuration utility for Sick LD LIDARs. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * ConfigFile was written by Richard J. Wagner 9 | * Download at http://www-personal.umich.edu/~wagnerr/ChE.html 10 | * 11 | * LDConfig is a simple utility to make configuring the SICK LD-LRS 12 | * easier. By editing the configuration file, one can set up the LD-LRS 13 | * in different configurations, without mucking about in source code. 14 | * 15 | * The Sick LIDAR Matlab/C++ Toolbox 16 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 17 | * All rights reserved. 18 | * 19 | * This software is released under a BSD Open-Source License. 20 | * See http://sicktoolbox.sourceforge.net 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include "ConfigFile.h" 30 | 31 | #define INVALID_OPTION_STRING " Invalid option!!! :o(" 32 | #define PROMPT_STRING "ld?> " 33 | 34 | /* Config file parameters */ 35 | #define CONFIG_OPT_MOTOR_SPD_STR "SICK_LD_MOTOR_SPEED" 36 | #define CONFIG_OPT_SCAN_AREA_STR "SICK_LD_SCAN_AREAS" 37 | #define CONFIG_OPT_SCAN_RES_STR "SICK_LD_SCAN_RESOLUTION" 38 | 39 | using namespace std; 40 | using namespace SickToolbox; 41 | 42 | /** 43 | * \fn sigintHandler 44 | * \brief Callback for SIGINT interrupt. Used for uninitialization. 45 | * \param signal Signal ID 46 | */ 47 | void sigintHandler(int signal); 48 | 49 | /** 50 | * \fn getUserOption 51 | * \brief A utility function for capturing input from the user 52 | * \param is_null_input Indicates whether the user entered null input 53 | * \return User option as an integer 54 | */ 55 | int getUserOption(bool &is_null_input); 56 | 57 | /** 58 | * \fn getFilename 59 | * \brief Prompts the user for config filename 60 | * \return Configuration file path 61 | */ 62 | string getFilename(); 63 | 64 | /** 65 | * \fn setConfig 66 | * \brief Attempts to set the desired config via the driver. 67 | * Parses configuration file as well. 68 | */ 69 | void setConfig(); 70 | 71 | /** 72 | * \fn printConfig 73 | * \brief Prints the current Sick status/config 74 | */ 75 | void printConfig(); 76 | 77 | /** 78 | * \fn parseScanAreasStr 79 | * \brief Parses the scan areas input string. 80 | * \param start_angs Array of angles to begin a sector 81 | * \param stop_angs Array of angles to end a sector 82 | * \param areas String of sectors (e.g. "[0 10.75] [40.5 60.0] [85 100]") 83 | * \param num_pairs Number of angle pairs found 84 | * \return true on success, false otherwise 85 | * 86 | * pre: all parameters != NULL 87 | * post: start_angs and stop_angs hold num_pairs values; 88 | * 0 < num_pairs < SickLD::SICK_MAX_NUM_MEASURING_SECTORS 89 | * 90 | * This function separates the scan areas found in the config file 91 | * into two arrays - one with angles that the scan areas begin at, 92 | * and another with the angles that the scan areas end at. Uses 93 | * setAngle() 94 | */ 95 | bool parseScanAreasStr(string& scan_areas_str, double *start_angs, double *stop_angs, int& num_sectors); 96 | 97 | /** 98 | * \fn parseNumStr 99 | * \param entry String to be converted to a number 100 | * \param num Variable to receive converted string 101 | * \return true on success, false otherwise 102 | * 103 | * Parses entry string into a double, and saves it in num. 104 | */ 105 | bool parseNumStr(const string& entry, double& num); 106 | 107 | /* A pointer to the current driver instance */ 108 | SickLD *sick_ld = NULL; 109 | 110 | int main(int argc, char* argv[]) 111 | { 112 | 113 | string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS); // IP address of the Sick LD unit 114 | 115 | /* Check the num args */ 116 | if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 117 | cerr << "Usage: ld_config [SICK IP ADDRESS]" << endl 118 | << "Ex. ld_config 192.168.1.11" << endl; 119 | return -1; 120 | } 121 | 122 | /* Assign the IP address */ 123 | if(argc == 2) { 124 | sick_ip_addr = argv[1]; 125 | } 126 | 127 | /* Instantiate the SickLD driver */ 128 | sick_ld = new SickLD(sick_ip_addr); 129 | 130 | cout << endl; 131 | cout << "The Sick LIDAR C++/Matlab Toolbox " << endl; 132 | cout << "Sick LD Config Utility " << endl; 133 | cout << endl; 134 | 135 | /* Initialize the Sick LD */ 136 | try { 137 | sick_ld->Initialize(); 138 | } 139 | 140 | catch(...) { 141 | cerr << "Initialize failed! Are you using the correct IP address?" << endl; 142 | return -1; 143 | } 144 | 145 | /* Register the signal handler */ 146 | signal(SIGINT,sigintHandler); 147 | 148 | do { 149 | 150 | cout << "Enter your choice: (Ctrl-c to exit)" << endl; 151 | cout << " [1] Set new configuration"<< endl; 152 | cout << " [2] Show current settings"<< endl; 153 | cout << PROMPT_STRING; 154 | 155 | bool is_null_input; 156 | switch(getUserOption(is_null_input)) { 157 | 158 | case 1: 159 | setConfig(); 160 | break; 161 | case 2: 162 | printConfig(); 163 | break; 164 | default: 165 | if(!is_null_input) { 166 | cerr << INVALID_OPTION_STRING << endl; 167 | } 168 | 169 | } 170 | 171 | cout << endl; 172 | 173 | } while(true); 174 | 175 | /* Success */ 176 | return 0; 177 | 178 | } 179 | 180 | /** 181 | * Handles the SIGINT signal 182 | */ 183 | void sigintHandler(int signal) { 184 | 185 | cout << endl; 186 | cout << "Quitting..." << endl; 187 | 188 | /* Unitialize the device */ 189 | try { 190 | 191 | sick_ld->Uninitialize(); 192 | delete sick_ld; 193 | 194 | } 195 | 196 | catch(...) { 197 | cerr << "Uninitialize failed!" << endl; 198 | exit(-1); 199 | } 200 | 201 | cout << endl; 202 | cout << "Thanks for using the Sick LIDAR Matlab/C++ Toolbox!" << endl; 203 | cout << "Bye Bye :o)" << endl; 204 | 205 | exit(0); 206 | 207 | } 208 | 209 | /** 210 | * Reads user input string and returns numeric representation. 211 | */ 212 | int getUserOption(bool &is_null_input) { 213 | 214 | string user_input_str; 215 | getline(cin,user_input_str); 216 | 217 | // Check whether its null input 218 | is_null_input = true; 219 | if (user_input_str.length() > 0) { 220 | is_null_input = false; 221 | } 222 | 223 | int int_val = 0; 224 | istringstream input_stream(user_input_str); 225 | input_stream >> int_val; 226 | 227 | return int_val; 228 | 229 | } 230 | 231 | /** 232 | * Prompts the user for the filename 233 | */ 234 | string getFilename() { 235 | 236 | string filename; 237 | 238 | cout << "config file: "; 239 | getline(cin,filename); 240 | 241 | return filename; 242 | 243 | } 244 | 245 | /** 246 | * Parses config file and loads new configuration 247 | */ 248 | void setConfig() { 249 | 250 | int motor_spd; 251 | int num_sectors; 252 | double scan_res; 253 | double start_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0}; 254 | double stop_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0}; 255 | string scan_areas_str; 256 | 257 | ConfigFile sick_config_file; // Extracts values from config file 258 | string config_path; 259 | 260 | /* Prompt the user for the filename */ 261 | config_path = getFilename(); 262 | 263 | /* Instantiate the parser */ 264 | if(ifstream(config_path.c_str())) { 265 | sick_config_file = ConfigFile(config_path); 266 | } 267 | else { 268 | cerr << "Invalid filename!" << endl; 269 | return; 270 | } 271 | 272 | /* Use the ConfigFile class to extract the various parameters for 273 | * the sick configuration. 274 | * 275 | * NOTE: The third parameter specifies the value to use, if the second 276 | * parameter is not found in the file. 277 | */ 278 | if(!sick_config_file.readInto(motor_spd,CONFIG_OPT_MOTOR_SPD_STR)) { 279 | cerr << "ERROR: Invalid config file - " << CONFIG_OPT_MOTOR_SPD_STR << " needs to be specified!" << endl; 280 | return; 281 | } 282 | 283 | if(!sick_config_file.readInto(scan_res,CONFIG_OPT_SCAN_RES_STR)) { 284 | cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_RES_STR << " needs to be specified!" << endl; 285 | return; 286 | } 287 | 288 | if(!sick_config_file.readInto(scan_areas_str,CONFIG_OPT_SCAN_AREA_STR)) { 289 | cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_AREA_STR << " needs to be specified!" << endl; 290 | return; 291 | } 292 | 293 | /* Extract the start/stop pairs and angle res for the scanning sectors */ 294 | if (!parseScanAreasStr(scan_areas_str,start_angs,stop_angs,num_sectors)) { 295 | cerr << "ERROR: Parser failed to extract scan areas!" << endl; 296 | return; 297 | } 298 | 299 | /* Set the global parameters - we pass them all at once to ensure a feasible config */ 300 | cout << endl << "\tAttempting to configure the Sick LD..." << endl; 301 | 302 | try { 303 | sick_ld->SetSickGlobalParamsAndScanAreas(motor_spd,scan_res,start_angs,stop_angs,num_sectors); 304 | } 305 | 306 | catch(SickConfigException &sick_config_exception) { 307 | cerr << "ERROR: Couldn't set requested configuration!" << endl; 308 | return; 309 | } 310 | 311 | catch(...) { 312 | exit(-1); 313 | } 314 | 315 | cout << "\t\tConfiguration Successfull!!!" << endl; 316 | 317 | } 318 | 319 | /** 320 | * Print the current Sick LD configuration 321 | */ 322 | void printConfig() { 323 | 324 | cout << endl; 325 | sick_ld->PrintSickStatus(); 326 | sick_ld->PrintSickIdentity(); 327 | sick_ld->PrintSickGlobalConfig(); 328 | sick_ld->PrintSickEthernetConfig(); 329 | sick_ld->PrintSickSectorConfig(); 330 | 331 | } 332 | 333 | /** 334 | * Parses scan areas string in order extract desired sector configuration 335 | */ 336 | bool parseScanAreasStr(string& areas, double * start_angs, double * stop_angs, int& num_pairs) { 337 | 338 | unsigned long i; // number of sectors found so far 339 | unsigned int start_pos = 0; // starting position of a sector in 'areas' 340 | unsigned int end_pos = 0; // ending position of a sector in 'areas' 341 | unsigned int split = 0; // position of the delimiter inside a sector 342 | string pair; // a string of 'areas' that contains one start and one stop angle 343 | 344 | /* Get the beginning and end of the first scan sector */ 345 | start_pos = areas.find('[',start_pos); // Find the next [, starting at start_pos 346 | end_pos = areas.find(']',start_pos); 347 | 348 | /* Keep getting sectors until we either run out of open 349 | * brackets or we exceed the number of allowed sectors 350 | */ 351 | for(i=0; (start_pos != (unsigned int)string::npos) && (i <= SickLD::SICK_MAX_NUM_MEASURING_SECTORS); i++) { 352 | 353 | pair = areas.substr(start_pos+1,end_pos-(start_pos+1)); 354 | 355 | /* Eliminate any padding before first value */ 356 | try { 357 | pair = pair.substr(pair.find_first_not_of(' ')); 358 | } 359 | 360 | catch(...) { 361 | cerr << "ERROR: There was an problem parsing your scan areas! Check your config file." << endl; 362 | return false; 363 | } 364 | 365 | split = pair.find(' '); 366 | 367 | /* Catch a lack of ' ' inside the sector definition; 368 | * alternative is to let it fail at pair.substr(split) 369 | */ 370 | if(split == (unsigned int)string::npos) { 371 | cerr << "ERROR: Invalid sector definition." << endl; 372 | return false; 373 | } 374 | 375 | /* Get the number from the beginning to the delimiter */ 376 | if(!parseNumStr(pair.substr(0,split),start_angs[i])) { 377 | cerr << "ERROR: Invalid start angle found." << endl; 378 | return false; 379 | } 380 | 381 | /* Get the number from the delimiter to the end */ 382 | if(!parseNumStr(pair.substr(split),stop_angs[i])) { 383 | cerr << "ERROR: Invalid stop angle found." << endl; 384 | return false; 385 | } 386 | 387 | start_pos = end_pos; // Shift to the end of the current sector 388 | 389 | /* Try to find another sector */ 390 | start_pos = areas.find("[", start_pos); 391 | end_pos = areas.find("]", start_pos); 392 | 393 | } 394 | 395 | num_pairs = i; 396 | 397 | /* Check if we broke out because of too many loops */ 398 | if(num_pairs > SickLD::SICK_MAX_NUM_MEASURING_SECTORS) { 399 | cerr << "ERROR: Too many scan areas found (max " << SickLD::SICK_MAX_NUM_MEASURING_SECTORS << ")" << endl; 400 | return false; 401 | } 402 | 403 | /* Or, we might not have even entered the loop */ 404 | if(num_pairs == 0) { 405 | cerr << "ERROR: No scan areas found! Check brackets in your config file." << endl; 406 | return false; 407 | } 408 | 409 | /* Success! */ 410 | return true; 411 | 412 | } 413 | 414 | /** 415 | * Parses entry string into a double, and saves it in num. 416 | */ 417 | bool parseNumStr(const string& entry, double& num) 418 | { 419 | 420 | string num_str = entry.substr(entry.find_first_not_of(' ')); 421 | istringstream input_stream(num_str.c_str()); 422 | if(!(input_stream >> num)) { 423 | cerr << "ERROR: Invalid angle value: " + num_str << endl; 424 | return false; 425 | } 426 | 427 | /* Success! */ 428 | return true; 429 | 430 | } 431 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_more_config/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_more_config/src/main.cc: -------------------------------------------------------------------------------- 1 | 2 | /*! 3 | * \file main.cc 4 | * \brief Illustrates a variety of ways to configure the flash 5 | * parameters on the Sick LD device as well as how to set 6 | * the unit's clock. 7 | * 8 | * Code by Jason C. Derenick and Thomas H. Miller. 9 | * Contact derenick(at)lehigh(dot)edu 10 | * 11 | * The Sick LIDAR Matlab/C++ Toolbox 12 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 13 | * All rights reserved. 14 | * 15 | * This software is released under a BSD Open-Source License. 16 | * See http://sicktoolbox.sourceforge.net 17 | */ 18 | 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | using namespace std; 25 | using namespace SickToolbox; 26 | 27 | int main (int argc, char *argv[]) { 28 | 29 | /* A string for the IP address */ 30 | string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS); 31 | 32 | /* Check the num of args */ 33 | if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 34 | cerr << "Usage: ld_more_config [SICK IP ADDRESS]" << endl 35 | << "Ex. ld_more_config 192.168.1.11" << endl; 36 | return -1; 37 | } 38 | 39 | /* Assign the IP address */ 40 | if(argc == 2) { 41 | sick_ip_addr = argv[1]; 42 | } 43 | 44 | /* Define the object */ 45 | SickLD sick_ld(sick_ip_addr); 46 | 47 | /* 48 | * Initialize the Sick LD 49 | */ 50 | try { 51 | sick_ld.Initialize(); 52 | } 53 | 54 | catch(...) { 55 | cerr << "Initialize failed! Are you using the correct IP address?" << endl; 56 | return -1; 57 | } 58 | 59 | try { 60 | 61 | /* Assign absolute and then relative time */ 62 | //uint16_t new_sick_time = 0; 63 | //sick_ld.SetSickTimeAbsolute(1500,new_sick_time); 64 | //cout << "\tNew sick time: " << new_sick_time << endl; 65 | //sick_ld.SetSickTimeRelative(-500,new_sick_time); 66 | //cout << "\tNew sick time: " << new_sick_time << endl; 67 | 68 | /* Configure the Sick LD sensor ID */ 69 | //sick_ld.PrintSickGlobalConfig(); 70 | //sick_ld.SetSickSensorID(16); 71 | //sick_ld.PrintSickGlobalConfig(); 72 | 73 | /* Configure the sick motor speed */ 74 | //sick_ld.PrintSickGlobalConfig(); 75 | //sick_ld.SetSickMotorSpeed(10); 76 | //sick_ld.PrintSickGlobalConfig(); 77 | 78 | /* Configure the sick scan resolution */ 79 | //sick_ld.PrintSickGlobalConfig(); 80 | //sick_ld.SetSickScanResolution(0.5); 81 | //sick_ld.PrintSickGlobalConfig(); 82 | 83 | /* Configure all the global parameters */ 84 | //double start_angle = 45; 85 | //double stop_angle = 315; 86 | //sick_ld.PrintSickGlobalConfig(); 87 | //sick_ld.PrintSickSectorConfig(); 88 | //sick_ld.SetSickGlobalParamsAndScanAreas(10,0.5,&start_angle,&stop_angle,1); 89 | //sick_ld.PrintSickGlobalConfig(); 90 | //sick_ld.PrintSickSectorConfig(); 91 | 92 | } 93 | 94 | catch(...) { 95 | cerr << "An error occurred!" << endl; 96 | } 97 | 98 | /* 99 | * Uninitialize the device 100 | */ 101 | try { 102 | sick_ld.Uninitialize(); 103 | } 104 | 105 | catch(...) { 106 | cerr << "Uninitialize failed!" << endl; 107 | return -1; 108 | } 109 | 110 | /* Success! */ 111 | return 0; 112 | 113 | } 114 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_multi_sector/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_multi_sector/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief Illustrates how to work with the Sick LD 4 | * using multiple scan areas/sectors. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /* Define the number of active sectors */ 23 | #define NUM_ACTIVE_SECTORS (3) 24 | 25 | using namespace std; 26 | using namespace SickToolbox; 27 | 28 | int main (int argc, char *argv[]) { 29 | 30 | /* A string for the IP address */ 31 | string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS); 32 | 33 | /* Check the num of args */ 34 | if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 35 | cerr << "Usage: ld_multi_sector [SICK IP ADDRESS]" << endl 36 | << "Ex. ld_multi_sector 192.168.1.11" << endl; 37 | return -1; 38 | } 39 | 40 | /* Assign the IP address */ 41 | if(argc == 2) { 42 | sick_ip_addr = argv[1]; 43 | } 44 | 45 | /* Define the temporal scan area (3 active sectors/scan areas) 46 | * NOTE: This scan configuration will persist until the power 47 | * is cycled or until it is reset w/ different params. 48 | */ 49 | double sector_start_angs[NUM_ACTIVE_SECTORS] = {45,270,345}; 50 | double sector_stop_angs[NUM_ACTIVE_SECTORS] = {90,315,15}; 51 | 52 | /* Define the destination buffers */ 53 | double range_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0}; 54 | unsigned int reflect_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0}; 55 | 56 | /* Some buffer for additional info we can get from the Sick LD */ 57 | unsigned int num_values[NUM_ACTIVE_SECTORS] = {0}; 58 | unsigned int data_offsets[NUM_ACTIVE_SECTORS] = {0}; 59 | unsigned int sector_ids[NUM_ACTIVE_SECTORS] = {0}; 60 | 61 | /* Define the object */ 62 | SickLD sick_ld(sick_ip_addr); 63 | 64 | /* Initialize the Sick LD */ 65 | try { 66 | sick_ld.Initialize(); 67 | } 68 | 69 | catch(...) { 70 | cerr << "Initialize failed! Are you using the correct IP address?" << endl; 71 | return -1; 72 | } 73 | 74 | try { 75 | 76 | /* Set the temporary scan areas */ 77 | sick_ld.SetSickTempScanAreas(sector_start_angs,sector_stop_angs,NUM_ACTIVE_SECTORS); 78 | 79 | /* Print the sector configuration */ 80 | sick_ld.PrintSickSectorConfig(); 81 | 82 | /* Request some measurements */ 83 | for (unsigned int i = 0; i < 10; i++) { 84 | 85 | /* Acquire the most recent range and reflectivity (echo amplitude) values */ 86 | sick_ld.GetSickMeasurements(range_values,reflect_values,num_values,sector_ids,data_offsets); 87 | 88 | /* Print out some data for each sector */ 89 | for (unsigned int i = 0; i < NUM_ACTIVE_SECTORS; i++) { 90 | 91 | cout << "\t[Sector ID: " << sector_ids[i] 92 | << ", Num Meas: " << num_values[i] 93 | << ", 1st Range Val: " << range_values[data_offsets[i]] 94 | << ", 1st Reflect Val: " << reflect_values[data_offsets[i]] 95 | << "]" << endl; 96 | 97 | } 98 | cout << endl; 99 | 100 | } 101 | 102 | } 103 | 104 | /* Catch any exception! */ 105 | catch(...) { 106 | cerr << "An error occurred!" << endl; 107 | } 108 | 109 | /* 110 | * Uninitialize the device 111 | */ 112 | try { 113 | sick_ld.Uninitialize(); 114 | } 115 | 116 | catch(...) { 117 | cerr << "Uninitialize failed!" << endl; 118 | return -1; 119 | } 120 | 121 | /* Success !*/ 122 | return 0; 123 | 124 | } 125 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_single_sector/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/ld/ld_single_sector/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple application illustrating the use of 4 | * the Sick LD C++ driver using a single sector. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | /* Use the namespace */ 23 | using namespace std; 24 | using namespace SickToolbox; 25 | 26 | int main (int argc, char *argv[]) { 27 | 28 | /* A string for the IP address */ 29 | string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS); 30 | 31 | /* Check the num of args */ 32 | if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 33 | cerr << "Usage: ld_single_sector [SICK IP ADDRESS]" << endl 34 | << "Ex. ld_single_sector 192.168.1.11" << endl; 35 | return -1; 36 | } 37 | 38 | /* Assign the IP address */ 39 | if(argc == 2) { 40 | sick_ip_addr = argv[1]; 41 | } 42 | 43 | /* Define the data buffers */ 44 | double values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0}; 45 | unsigned int num_values = 0; 46 | 47 | /* Define the bounds for a single sector */ 48 | double sector_start_ang = 90; 49 | double sector_stop_ang = 270; 50 | 51 | /* Define the object */ 52 | SickLD sick_ld(sick_ip_addr); 53 | 54 | /* 55 | * Initialize the Sick LD 56 | */ 57 | try { 58 | sick_ld.Initialize(); 59 | } 60 | 61 | catch(...) { 62 | cerr << "Initialize failed! Are you using the correct IP address?" << endl; 63 | return -1; 64 | } 65 | 66 | try { 67 | 68 | /* Set the desired sector configuration */ 69 | sick_ld.SetSickTempScanAreas(§or_start_ang,§or_stop_ang,1); 70 | 71 | /* Print the sector configuration */ 72 | sick_ld.PrintSickSectorConfig(); 73 | 74 | /* Acquire some range measurements */ 75 | for (unsigned int i = 0; i < 10; i++) { 76 | 77 | /* Here we only want the range values so the second arg is NULL */ 78 | sick_ld.GetSickMeasurements(values,NULL,&num_values); 79 | cout << "\t Num. Values: " << num_values << endl; 80 | 81 | } 82 | 83 | } 84 | 85 | /* Catch any exceptions */ 86 | catch(...) { 87 | cerr << "An error occurred!" << endl; 88 | } 89 | 90 | /* 91 | * Uninitialize the device 92 | */ 93 | try { 94 | sick_ld.Uninitialize(); 95 | } 96 | 97 | catch(...) { 98 | cerr << "Uninitialize failed!" << endl; 99 | return -1; 100 | } 101 | 102 | /* Success !*/ 103 | return 0; 104 | 105 | } 106 | -------------------------------------------------------------------------------- /c++/examples/lms1xx/lms1xx_simple_app/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms1xx/lms1xx_simple_app/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple application using the Sick LMS 1xx driver. 4 | * 5 | * Code by Jason C. Derenick and Christopher R. Mansley. 6 | * Contact jasonder(at)seas(dot)upenn(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | using namespace SickToolbox; 23 | 24 | int main(int argc, char* argv[]) 25 | { 26 | 27 | /* 28 | * Instantiate an instance 29 | */ 30 | SickLMS1xx sick_lms_1xx; 31 | 32 | /* 33 | * Initialize the Sick LMS 2xx 34 | */ 35 | try { 36 | sick_lms_1xx.Initialize(); 37 | } 38 | 39 | catch(...) { 40 | cerr << "Initialize failed! Are you using the correct IP address?" << endl; 41 | return -1; 42 | } 43 | 44 | try { 45 | unsigned int status = 1; 46 | unsigned int num_measurements = 0; 47 | unsigned int range_1_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS]; 48 | unsigned int range_2_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS]; 49 | //sick_lms_1xx.SetSickScanFreqAndRes(SickLMS1xx::SICK_LMS_1XX_SCAN_FREQ_25, 50 | //SickLMS1xx::SICK_LMS_1XX_SCAN_RES_25); 51 | //sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_DIST_DOUBLE_PULSE, 52 | // SickLMS1xx::SICK_LMS_1XX_REFLECT_NONE); 53 | sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT); 54 | for (int i = 0; i < 1000; i++) { 55 | sick_lms_1xx.GetSickMeasurements(range_1_vals,range_2_vals,range_1_vals,range_2_vals,num_measurements,&status); 56 | std::cout << i << ": " << num_measurements << " " << status << std::endl; 57 | } 58 | } 59 | 60 | catch(SickConfigException sick_exception) { 61 | std::cout << sick_exception.what() << std::endl; 62 | } 63 | 64 | catch(SickIOException sick_exception) { 65 | std::cout << sick_exception.what() << std::endl; 66 | } 67 | 68 | catch(SickTimeoutException sick_exception) { 69 | std::cout << sick_exception.what() << std::endl; 70 | } 71 | 72 | catch(...) { 73 | cerr << "An Error Occurred!" << endl; 74 | return -1; 75 | } 76 | 77 | 78 | /* 79 | * Uninitialize the device 80 | */ 81 | try { 82 | sick_lms_1xx.Uninitialize(); 83 | } 84 | 85 | catch(...) { 86 | cerr << "Uninitialize failed!" << endl; 87 | return -1; 88 | } 89 | 90 | /* Success! */ 91 | return 0; 92 | 93 | } 94 | 95 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_config/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_config 6 | Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o) 7 | 8 | Desc: This example provides a simple shell-like configuration 9 | utility for working w/ Sick LMS 2xx LIDARs. It can be 10 | used for setting measuring units, measuring mode, and 11 | sensitivity. It can also display the current LMS config. 12 | 13 | Example call (from build dir): ./lms_config /dev/ttyUSB0 9600 14 | 15 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_config/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_mean_values/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_mean_values 6 | Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o) 7 | 8 | Desc: This example illustrates grabbing mean values from the Sick LMS. 9 | 10 | Example call (from build dir): ./lms_mean_values /dev/ttyUSB0 9600 11 | 12 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_mean_values/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_mean_values/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple example illustrating how to acquire 4 | * mean values from the Sick LMS 2xx. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | using namespace SickToolbox; 24 | 25 | int main(int argc, char* argv[]) 26 | { 27 | 28 | string device_str; 29 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 30 | 31 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh 32 | unsigned int num_values; // Holds the number of measurements returned 33 | 34 | /* Check for a device path. If it's not present, print a usage statement. */ 35 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 36 | cout << "Usage: lms2xx_mean_values PATH [BAUD RATE]" << endl 37 | << "Ex: lms2xx_mean_values /dev/ttyUSB0 9600" << endl; 38 | return -1; 39 | } 40 | 41 | /* Only device path is given */ 42 | if (argc == 2) { 43 | device_str = argv[1]; 44 | } 45 | 46 | /* Device path and baud are given */ 47 | if (argc == 3) { 48 | device_str = argv[1]; 49 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 50 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 51 | return -1; 52 | } 53 | } 54 | 55 | /* 56 | * Instantiate an instance 57 | */ 58 | SickLMS2xx sick_lms_2xx(device_str); 59 | 60 | /* 61 | * Initialize the Sick LMS 2xx 62 | */ 63 | try { 64 | sick_lms_2xx.Initialize(desired_baud); 65 | } 66 | 67 | catch(...) { 68 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 69 | return -1; 70 | } 71 | 72 | /* 73 | * Acquire a few scans from the Sick LMS 74 | */ 75 | try { 76 | 77 | for (unsigned int i=0; i < 10; i++) { 78 | 79 | /* 80 | * The first argument is an input to the driver 81 | * telling it the number of raw scans over which 82 | * the Sick should average. (i.e. here, the Sick 83 | * will average over 5 scans before returning 84 | * the resulting mean values) 85 | */ 86 | sick_lms_2xx.GetSickMeanValues(5,values,num_values); 87 | cout << "\t Num. Values: " << num_values << endl; 88 | 89 | } 90 | 91 | } 92 | 93 | /* Catch anything else and exit */ 94 | catch(...) { 95 | cerr << "An error occurred!" << endl; 96 | } 97 | 98 | /* 99 | * Uninitialize the device 100 | */ 101 | try { 102 | sick_lms_2xx.Uninitialize(); 103 | } 104 | 105 | catch(...) { 106 | cerr << "Uninitialize failed!" << endl; 107 | return -1; 108 | } 109 | 110 | /* Success! */ 111 | return 0; 112 | 113 | } 114 | 115 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_partial_scan/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_partial_scan 6 | Note: This example is NOT COMPATIBLE w/ LMS {211,221,291}-S14 LIDARs! 7 | 8 | Desc: This example illustrates grabbing partial scans using the 9 | Sick LMS driver. 10 | 11 | Example call (from build dir): ./lms_partial_scan /dev/ttyUSB0 9600 12 | 13 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_partial_scan/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_partial_scan/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief Illustrates how to acquire partial scans 4 | * from the Sick LMS as well as telegram indices. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace SickToolbox; 25 | 26 | int main(int argc, char* argv[]) { 27 | 28 | string device_str; 29 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 30 | 31 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh 32 | unsigned int num_values = 0; // Holds the number of measurements returned 33 | unsigned int scan_idx = 0; // Holds the idx for the returned partial scan 34 | unsigned int telegram_idx = 0; // Holds the idx of the telegram associated w/ scan 35 | 36 | /* Check for a device path. If it's not present, print a usage statement. */ 37 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 38 | cout << "Usage: lms2xx_partial_scan PATH [BAUD RATE]" << endl 39 | << "Ex: lms2xx_partial_scan /dev/ttyUSB0 9600" << endl; 40 | return -1; 41 | } 42 | 43 | /* Only device path is given */ 44 | if (argc == 2) { 45 | device_str = argv[1]; 46 | } 47 | 48 | /* Device path and baud are given */ 49 | if (argc == 3) { 50 | device_str = argv[1]; 51 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 52 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 53 | return -1; 54 | } 55 | } 56 | 57 | /* 58 | * Instantiate an instance 59 | */ 60 | SickLMS2xx sick_lms_2xx(device_str); 61 | 62 | /* 63 | * Initialize the Sick LMS 2xx 64 | */ 65 | try { 66 | sick_lms_2xx.Initialize(desired_baud); 67 | } 68 | 69 | catch(...) { 70 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 71 | return -1; 72 | } 73 | 74 | /* 75 | * Ensure real-time indices are set 76 | */ 77 | if (sick_lms_2xx.GetSickAvailability() & SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES) { 78 | 79 | try { 80 | 81 | /* 82 | * Set the device variant to 100/0.25 83 | * 84 | * NOTE: Setting the variant this way ensures that the 85 | * partial scans will start at angles that are a 86 | * multiple of 0.25 deg. 87 | * 88 | */ 89 | sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_100,SickLMS2xx::SICK_SCAN_RESOLUTION_25); 90 | 91 | /* 92 | * Acquire some partial scans from Sick 93 | */ 94 | for (unsigned int i=0; i < 12; i++) { 95 | 96 | /* 97 | * NOTE: Notice that here we are also obtaining the telegram idx. In a 98 | * proper implementation, this value would be used to ensure the 99 | * temporal consistency (acting as a sequence number) of the newly 100 | * obtained partial scan. Here we simply just print it out. 101 | * 102 | * Also, we don't want Field A,B, or C outputs... so we pass in 103 | * NULL for these params. 104 | */ 105 | sick_lms_2xx.GetSickPartialScan(values,num_values,scan_idx,NULL,NULL,NULL,&telegram_idx); 106 | cout << "\t Start angle: " << setw(4) << 0.25*scan_idx << ", Num. Values: " << num_values << ", Msg Idx: " << telegram_idx << endl; 107 | 108 | } 109 | 110 | } 111 | 112 | /* Catch anything else and exit */ 113 | catch(...) { 114 | cerr << "An error occurred!" << endl; 115 | } 116 | 117 | } 118 | 119 | else { 120 | cout << "Please set the Sick LMS to an availability w/ real-time indices..." << endl; 121 | cout << "Hint: Use the lms_config utility/example! :o)"<< endl; 122 | } 123 | 124 | /* 125 | * Uninitialize the device 126 | */ 127 | try { 128 | sick_lms_2xx.Uninitialize(); 129 | } 130 | 131 | catch(...) { 132 | cerr << "Uninitialize failed!" << endl; 133 | return -1; 134 | } 135 | 136 | /* Success! */ 137 | return 0; 138 | 139 | } 140 | 141 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_real_time_indices/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_real_time_indices 6 | Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o) 7 | 8 | Desc: This example provides a simple program template for grabbing 9 | data as well as real-time indices from a Sick LMS 2xx LIDAR. 10 | 11 | Example call (from build dir): ./lms_real_time_indices /dev/ttyUSB0 9600 12 | 13 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_real_time_indices/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_real_time_indices/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief Illustrates how to use the Sick Toolbox C++ interface 4 | * to acquire scan data along with real-time indices from 5 | * a Sick LMS 2xx. 6 | * 7 | * Code by Jason C. Derenick and Thomas H. Miller. 8 | * Contact derenick(at)lehigh(dot)edu 9 | * 10 | * The Sick LIDAR Matlab/C++ Toolbox 11 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 12 | * All rights reserved. 13 | * 14 | * This software is released under a BSD Open-Source License. 15 | * See http://sicktoolbox.sourceforge.net 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | #include 22 | 23 | using namespace std; 24 | using namespace SickToolbox; 25 | 26 | int main(int argc, char* argv[]) 27 | { 28 | 29 | string device_str; 30 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 31 | 32 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh 33 | unsigned int num_values = 0; // Holds the number of measurements returned 34 | unsigned int telegram_idx = 0; 35 | unsigned int real_time_idx = 0; 36 | 37 | /* Check for a device path. If it's not present, print a usage statement. */ 38 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 39 | cout << "Usage: lms2xx_real_time_indices PATH [BAUD RATE]" << endl 40 | << "Ex: lms2xx_real_time_indices /dev/ttyUSB0 9600" << endl; 41 | return -1; 42 | } 43 | 44 | /* Only device path is given */ 45 | if (argc == 2) { 46 | device_str = argv[1]; 47 | } 48 | 49 | /* Device path and baud are given */ 50 | if (argc == 3) { 51 | device_str = argv[1]; 52 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 53 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 54 | return -1; 55 | } 56 | } 57 | 58 | /* 59 | * Instantiate an instance 60 | */ 61 | SickLMS2xx sick_lms_2xx(device_str); 62 | 63 | /* 64 | * Initialize the Sick LMS 2xx 65 | */ 66 | try { 67 | sick_lms_2xx.Initialize(desired_baud); 68 | } 69 | 70 | catch(...) { 71 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 72 | return -1; 73 | } 74 | 75 | /* 76 | * Ensure real-time indices are enabled 77 | */ 78 | if (sick_lms_2xx.GetSickAvailability() & SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES) { 79 | 80 | try { 81 | 82 | /* 83 | * Acquire a few scans from the Sick LMS 84 | */ 85 | for (unsigned int i=0; i < 10; i++) { 86 | 87 | /* We don't want Fields A,B, or C, so we pass NULL */ 88 | sick_lms_2xx.GetSickScan(values,num_values,NULL,NULL,NULL,&telegram_idx,&real_time_idx); 89 | cout << "\t Num. Values: " << num_values << ", Msg Idx: " << telegram_idx 90 | << ", Real-time Idx: " << real_time_idx << endl; 91 | 92 | } 93 | 94 | } 95 | 96 | /* Catch anything else and exit */ 97 | catch(...) { 98 | cerr << "An error occurred!" << endl; 99 | return -1; 100 | } 101 | 102 | } 103 | 104 | else { 105 | cout << "Please set the Sick LMS to an availability w/ real-time indices..." << endl; 106 | cout << "Hint: Use the lms_config utility/example! :o)"<< endl; 107 | } 108 | 109 | /* 110 | * Uninitialize the device 111 | */ 112 | try { 113 | sick_lms_2xx.Uninitialize(); 114 | } 115 | 116 | catch(...) { 117 | cerr << "Uninitialize failed!" << endl; 118 | return -1; 119 | } 120 | 121 | /* Success! */ 122 | return 0; 123 | 124 | } 125 | 126 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_set_variant/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_set_variant 6 | Note: This example is NOT COMPATIBLE w/ LMS {211,221,291}-S14 LIDARs! 7 | 8 | Desc: This example illustrates the proper way to switch the Sick LMS 9 | variant (i.e. the Sick's FOV and scan resolution). 10 | 11 | Example call (from build dir): ./lms_set_variant /dev/ttyUSB0 9600 12 | 13 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_set_variant/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_set_variant/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief Illustrates how to set the device variant and then 4 | * acquire measured values 5 | * 6 | * Note: This example WILL NOT WORK for LMS 211-S14, 221-S14, 7 | * 291-S14 models as they do not support variant switching. 8 | * 9 | * Code by Jason C. Derenick and Thomas H. Miller. 10 | * Contact derenick(at)lehigh(dot)edu 11 | * 12 | * The Sick LIDAR Matlab/C++ Toolbox 13 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 14 | * All rights reserved. 15 | * 16 | * This software is released under a BSD Open-Source License. 17 | * See http://sicktoolbox.sourceforge.net 18 | */ 19 | 20 | /* Implementation dependencies */ 21 | #include 22 | #include 23 | #include 24 | #include 25 | 26 | /* Use the namespace */ 27 | using namespace std; 28 | using namespace SickToolbox; 29 | 30 | int main(int argc, char * argv[]) { 31 | 32 | string device_str; 33 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 34 | 35 | /* Check for a device path. If it's not present, print a usage statement. */ 36 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 37 | cout << "Usage: lms2xx_set_variant PATH [BAUD RATE]" << endl 38 | << "Ex: lms2xx_set_variant /dev/ttyUSB0 9600" << endl; 39 | return -1; 40 | } 41 | 42 | /* Only device path is given */ 43 | if (argc == 2) { 44 | device_str = argv[1]; 45 | } 46 | 47 | /* Device path and baud are given */ 48 | if (argc == 3) { 49 | device_str = argv[1]; 50 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 51 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 52 | return -1; 53 | } 54 | } 55 | 56 | /* Instantiate the SickLMS2xx class with the device path string. */ 57 | SickLMS2xx sick_lms_2xx(device_str); 58 | 59 | /* Define some buffers to hold the returned measurements */ 60 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; 61 | unsigned int num_values = 0; 62 | 63 | /* 64 | * Initialize the Sick LMS 2xx 65 | */ 66 | try { 67 | sick_lms_2xx.Initialize(desired_baud); 68 | } 69 | 70 | catch(...) { 71 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 72 | return -1; 73 | } 74 | 75 | /* 76 | * Check whether device is LMS Fast 77 | */ 78 | if (!sick_lms_2xx.IsSickLMS2xxFast()) { 79 | 80 | try { 81 | 82 | /* 83 | * Set the device variant to 100/0.25 84 | * 85 | * NOTE: If an invalid variant definition is 86 | * given a SickConfigException will be 87 | * thrown stating so. 88 | * 89 | */ 90 | cout << "\tSetting variant to 100/0.25" << std::endl << flush; 91 | sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_100,SickLMS2xx::SICK_SCAN_RESOLUTION_25); 92 | 93 | /* 94 | * Acquire some measurements from Sick LMS 2xx using 100/0.25 95 | */ 96 | cout << "\tAcquiring some measurements..." << endl; 97 | for(unsigned int i = 0; i < 10; i++) { 98 | 99 | /* Acquire the most recent scan from the Sick */ 100 | sick_lms_2xx.GetSickScan(values,num_values); 101 | 102 | /* Display the number of measurements */ 103 | cout << "\t Num. Values: " << num_values << endl; 104 | 105 | } 106 | 107 | /* 108 | * Set the device variant to 180/0.5 109 | */ 110 | cout << std::endl << "\tSetting variant to 180/0.50" << endl; 111 | sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_180,SickLMS2xx::SICK_SCAN_RESOLUTION_50); 112 | 113 | /* 114 | * Acquire some measurements from Sick LMS 2xx using 180/0.50 115 | */ 116 | cout << "\tAcquiring some measurements..." << endl; 117 | for(unsigned int i = 0; i < 10; i++) { 118 | 119 | /* Acquire the most recent scan from the Sick */ 120 | sick_lms_2xx.GetSickScan(values,num_values); 121 | 122 | /* Display the number of measured values */ 123 | cout << "\t Num. Values: " << num_values << endl; 124 | 125 | } 126 | 127 | } 128 | 129 | catch(...) { 130 | cerr << "An error occurred!" << endl; 131 | } 132 | 133 | } 134 | 135 | else { 136 | cerr << "Oops... Your Sick is an LMS Fast!" << endl; 137 | cerr << "It doesn't support the variant command." << endl; 138 | } 139 | 140 | /* 141 | * Uninitialize the device 142 | */ 143 | try { 144 | sick_lms_2xx.Uninitialize(); 145 | } 146 | 147 | catch(...) { 148 | cerr << "Uninitialize failed!" << endl; 149 | return -1; 150 | } 151 | 152 | /* Success! */ 153 | return 0; 154 | 155 | } 156 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_simple_app/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_simple_app 6 | Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o) 7 | 8 | Desc: This example provides a simple program template for grabbing 9 | data from a Sick LMS 2xx LIDAR. It illustrates the simplest 10 | use of the Sick LIDAR Toolkit w/ C++. 11 | 12 | Example call (from build dir): ./lms_simple_app /dev/ttyUSB0 9600 13 | 14 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_simple_app/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_simple_app/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple application using the Sick LMS 2xx driver. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | using namespace std; 22 | using namespace SickToolbox; 23 | 24 | int main(int argc, char* argv[]) 25 | { 26 | 27 | string device_str; 28 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 29 | 30 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh 31 | unsigned int num_values = 0; // Holds the number of measurements returned 32 | 33 | /* Check for a device path. If it's not present, print a usage statement. */ 34 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 35 | cout << "Usage: lms2xx_simple_app PATH [BAUD RATE]" << endl 36 | << "Ex: lms2xx_simple_app /dev/ttyUSB0 9600" << endl; 37 | return -1; 38 | } 39 | 40 | /* Only device path is given */ 41 | if (argc == 2) { 42 | device_str = argv[1]; 43 | } 44 | 45 | /* Device path and baud are given */ 46 | if (argc == 3) { 47 | device_str = argv[1]; 48 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 49 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 50 | return -1; 51 | } 52 | } 53 | 54 | /* 55 | * Instantiate an instance 56 | */ 57 | SickLMS2xx sick_lms_2xx(device_str); 58 | 59 | /* 60 | * Initialize the Sick LMS 2xx 61 | */ 62 | try { 63 | sick_lms_2xx.Initialize(desired_baud); 64 | } 65 | 66 | catch(...) { 67 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 68 | return -1; 69 | } 70 | 71 | /* 72 | * Acquire a few scans from the Sick LMS 73 | */ 74 | try { 75 | 76 | for (unsigned int i=0; i < 10; i++) { 77 | sick_lms_2xx.GetSickScan(values,num_values); 78 | cout << "\t Num. Values: " << num_values << endl; 79 | } 80 | 81 | } 82 | 83 | /* Catch anything else and exit */ 84 | catch(...) { 85 | cerr << "An error occurred!" << endl; 86 | } 87 | 88 | /* 89 | * Uninitialize the device 90 | */ 91 | try { 92 | sick_lms_2xx.Uninitialize(); 93 | } 94 | 95 | catch(...) { 96 | cerr << "Uninitialize failed!" << endl; 97 | return -1; 98 | } 99 | 100 | /* Success! */ 101 | return 0; 102 | 103 | } 104 | 105 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_stream_range_and_reflect/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_stream_range_and_reflect 6 | Note: This example is COMPATIBLE w/ ONLY Sick 291-S14 (LMS FAST) LIDAR units! 7 | 8 | Desc: This example provides a simple program template for streaming 9 | range and reflectivity data from a Sick LMS 291-S14 LIDAR. 10 | 11 | Example call (from build dir): ./lms_stream_range_and_reflect /dev/ttyUSB0 9600 12 | 13 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple program illustrating how to stream range 4 | * and reflectivity returns from a Sick LMS 291-S14. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | using namespace SickToolbox; 24 | 25 | int main(int argc, char* argv[]) { 26 | 27 | string device_str; 28 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 29 | 30 | unsigned int num_range_values; 31 | unsigned int num_reflect_values; 32 | unsigned int range_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; 33 | unsigned int reflect_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; 34 | 35 | /* Check for a device path. If it's not present, print a usage statement. */ 36 | if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) { 37 | cout << "Usage: lms2xx_stream_range_and_reflect PATH [BAUD RATE]" << endl 38 | << "Ex: lms2xx_stream_range_and_reflect /dev/ttyUSB0 9600" << endl; 39 | return -1; 40 | } 41 | 42 | /* Only device path is given */ 43 | if (argc == 2) { 44 | device_str = argv[1]; 45 | } 46 | 47 | /* Device path and baud are given */ 48 | if (argc == 3) { 49 | device_str = argv[1]; 50 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 51 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 52 | return -1; 53 | } 54 | } 55 | 56 | /* 57 | * Instantiate driver instance 58 | */ 59 | SickLMS2xx sick_lms_2xx(device_str); 60 | 61 | /* 62 | * Initialize the Sick LMS 2xx 63 | */ 64 | try { 65 | sick_lms_2xx.Initialize(desired_baud); 66 | } 67 | 68 | catch(...) { 69 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 70 | return -1; 71 | } 72 | 73 | /* 74 | * Ensure it is an LMS Fast model 75 | */ 76 | if (sick_lms_2xx.IsSickLMS2xxFast()) { 77 | 78 | /* 79 | * Grab range and reflectivity data from the Sick LMS Fast 80 | */ 81 | try { 82 | 83 | for (unsigned int i=0; i < 10; i++) { 84 | sick_lms_2xx.GetSickScan(range_values,reflect_values,num_range_values,num_reflect_values); 85 | cout << "Num. Range Vals: " << num_range_values << " Num. Reflect Vals: " << num_reflect_values << endl; 86 | } 87 | 88 | } 89 | 90 | catch (...) { 91 | cerr << "An error occurred!" << endl; 92 | } 93 | 94 | } 95 | 96 | else { 97 | cerr << "Oops... Your Sick is NOT an LMS Fast!" << endl; 98 | cerr << "It doesn't support this kind of stream." << endl; 99 | } 100 | 101 | /* 102 | * Uninitialize the device 103 | */ 104 | try { 105 | sick_lms_2xx.Uninitialize(); 106 | } 107 | 108 | catch(...) { 109 | cerr << "Uninitialize failed!" << endl; 110 | return -1; 111 | } 112 | 113 | /* Success! */ 114 | return 0; 115 | 116 | } 117 | 118 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_subrange/README: -------------------------------------------------------------------------------- 1 | ================================================= 2 | Sick LIDAR Matlab/C++ Toolbox 3 | ================================================= 4 | 5 | Example: lms_subrange 6 | Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o) 7 | 8 | Desc: This example illustrates how to acquire a scan subrange from 9 | the Sick LMS. 10 | 11 | Example call (from build dir): ./lms_subrange /dev/ttyUSB0 9600 12 | 13 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_subrange/src/.deps/main.Po: -------------------------------------------------------------------------------- 1 | # dummy 2 | -------------------------------------------------------------------------------- /c++/examples/lms2xx/lms2xx_subrange/src/main.cc: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file main.cc 3 | * \brief A simple program illustrating how to acquire a scan 4 | * subrange from a Sick LMS 2xx. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | using namespace std; 23 | using namespace SickToolbox; 24 | 25 | int main(int argc, char* argv[]) 26 | { 27 | 28 | string device_str; 29 | SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400; 30 | 31 | unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh 32 | unsigned int num_values; // Holds the number of measurements returned 33 | 34 | /* Check for a device path. If it's not present, print a usage statement. */ 35 | if ((argc != 2 && argc != 3) || (strcasecmp(argv[1],"--help") == 0)) { 36 | cout << "Usage: lms2xx_subrange PATH [BAUD RATE]" << endl 37 | << "Ex: lms2xx_subrange /dev/ttyUSB0 9600" << endl; 38 | return -1; 39 | } 40 | 41 | /* Only device path is given */ 42 | if (argc == 2) { 43 | device_str = argv[1]; 44 | } 45 | 46 | /* Device path and baud are given */ 47 | if (argc == 3) { 48 | device_str = argv[1]; 49 | if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) { 50 | cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl; 51 | return -1; 52 | } 53 | } 54 | 55 | /* 56 | * Instantiate an instance 57 | */ 58 | SickLMS2xx sick_lms_2xx(device_str); 59 | 60 | /* 61 | * Initialize the Sick LMS 2xx 62 | */ 63 | try { 64 | sick_lms_2xx.Initialize(desired_baud); 65 | } 66 | 67 | catch(...) { 68 | cerr << "Initialize failed! Are you using the correct device path?" << endl; 69 | return -1; 70 | } 71 | 72 | try { 73 | 74 | /* 75 | * Acquire a few scans from the Sick LMS 76 | */ 77 | for (unsigned int i=0; i < 10; i++) { 78 | 79 | /* 80 | * Acquire the first ten measurements of a scan from the Sick 81 | */ 82 | sick_lms_2xx.GetSickScanSubrange(1,10,values,num_values); 83 | cout << "\t Num. Values: " << num_values << endl; 84 | 85 | } 86 | 87 | } 88 | 89 | catch(...) { 90 | cerr << "An error occurred!" << endl; 91 | } 92 | 93 | /* 94 | * Uninitialize the device 95 | */ 96 | try { 97 | sick_lms_2xx.Uninitialize(); 98 | } 99 | 100 | catch(...) { 101 | cerr << "Uninitialize failed!" << endl; 102 | return -1; 103 | } 104 | 105 | /* Success! */ 106 | return 0; 107 | 108 | } 109 | 110 | -------------------------------------------------------------------------------- /doxygen.cfg: -------------------------------------------------------------------------------- 1 | # Doxygen 1.5.1 2 | 3 | #--------------------------------------------------------------------------- 4 | # Project related configuration options 5 | #--------------------------------------------------------------------------- 6 | PROJECT_NAME = $(PROJECT)-$(VERSION) 7 | PROJECT_NUMBER = 8 | OUTPUT_DIRECTORY = $(DOCDIR) 9 | CREATE_SUBDIRS = NO 10 | OUTPUT_LANGUAGE = English 11 | USE_WINDOWS_ENCODING = NO 12 | BRIEF_MEMBER_DESC = YES 13 | REPEAT_BRIEF = YES 14 | ABBREVIATE_BRIEF = 15 | ALWAYS_DETAILED_SEC = NO 16 | INLINE_INHERITED_MEMB = NO 17 | FULL_PATH_NAMES = YES 18 | STRIP_FROM_PATH = $(SRCDIR) 19 | STRIP_FROM_INC_PATH = $(SRCDIR) 20 | SHORT_NAMES = YES 21 | JAVADOC_AUTOBRIEF = NO 22 | MULTILINE_CPP_IS_BRIEF = NO 23 | DETAILS_AT_TOP = NO 24 | INHERIT_DOCS = YES 25 | SEPARATE_MEMBER_PAGES = NO 26 | TAB_SIZE = 8 27 | ALIASES = 28 | OPTIMIZE_OUTPUT_FOR_C = NO 29 | OPTIMIZE_OUTPUT_JAVA = NO 30 | BUILTIN_STL_SUPPORT = NO 31 | DISTRIBUTE_GROUP_DOC = NO 32 | SUBGROUPING = YES 33 | 34 | #--------------------------------------------------------------------------- 35 | # Build related configuration options 36 | #--------------------------------------------------------------------------- 37 | EXTRACT_ALL = YES 38 | EXTRACT_PRIVATE = NO 39 | EXTRACT_STATIC = NO 40 | EXTRACT_LOCAL_CLASSES = YES 41 | EXTRACT_LOCAL_METHODS = NO 42 | HIDE_UNDOC_MEMBERS = NO 43 | HIDE_UNDOC_CLASSES = NO 44 | HIDE_FRIEND_COMPOUNDS = NO 45 | HIDE_IN_BODY_DOCS = NO 46 | INTERNAL_DOCS = NO 47 | CASE_SENSE_NAMES = YES 48 | HIDE_SCOPE_NAMES = NO 49 | SHOW_INCLUDE_FILES = YES 50 | INLINE_INFO = YES 51 | SORT_MEMBER_DOCS = YES 52 | SORT_BRIEF_DOCS = NO 53 | SORT_BY_SCOPE_NAME = NO 54 | GENERATE_TODOLIST = YES 55 | GENERATE_TESTLIST = YES 56 | GENERATE_BUGLIST = YES 57 | GENERATE_DEPRECATEDLIST= YES 58 | ENABLED_SECTIONS = 59 | MAX_INITIALIZER_LINES = 30 60 | SHOW_USED_FILES = YES 61 | SHOW_DIRECTORIES = NO 62 | FILE_VERSION_FILTER = 63 | 64 | #--------------------------------------------------------------------------- 65 | # configuration options related to warning and progress messages 66 | #--------------------------------------------------------------------------- 67 | QUIET = NO 68 | WARNINGS = YES 69 | WARN_IF_UNDOCUMENTED = YES 70 | WARN_IF_DOC_ERROR = YES 71 | WARN_NO_PARAMDOC = NO 72 | WARN_FORMAT = "$file:$line: $text" 73 | WARN_LOGFILE = 74 | 75 | #--------------------------------------------------------------------------- 76 | # configuration options related to the input files 77 | #--------------------------------------------------------------------------- 78 | INPUT = $(SRCDIR) 79 | FILE_PATTERNS = *.cc *.hh 80 | RECURSIVE = YES 81 | EXCLUDE = $(SRCDIR)/c++/examples 82 | EXCLUDE_SYMLINKS = NO 83 | EXCLUDE_PATTERNS = 84 | EXAMPLE_PATH = 85 | EXAMPLE_PATTERNS = 86 | EXAMPLE_RECURSIVE = NO 87 | IMAGE_PATH = 88 | INPUT_FILTER = 89 | FILTER_PATTERNS = 90 | FILTER_SOURCE_FILES = NO 91 | 92 | #--------------------------------------------------------------------------- 93 | # configuration options related to source browsing 94 | #--------------------------------------------------------------------------- 95 | SOURCE_BROWSER = NO 96 | INLINE_SOURCES = NO 97 | STRIP_CODE_COMMENTS = NO 98 | REFERENCED_BY_RELATION = YES 99 | REFERENCES_RELATION = YES 100 | REFERENCES_LINK_SOURCE = YES 101 | USE_HTAGS = NO 102 | VERBATIM_HEADERS = YES 103 | 104 | #--------------------------------------------------------------------------- 105 | # configuration options related to the alphabetical class index 106 | #--------------------------------------------------------------------------- 107 | ALPHABETICAL_INDEX = YES 108 | COLS_IN_ALPHA_INDEX = 5 109 | IGNORE_PREFIX = 110 | 111 | #--------------------------------------------------------------------------- 112 | # configuration options related to the HTML output 113 | #--------------------------------------------------------------------------- 114 | GENERATE_HTML = $(GENERATE_HTML) 115 | HTML_OUTPUT = html 116 | HTML_FILE_EXTENSION = .html 117 | HTML_HEADER = 118 | HTML_FOOTER = 119 | HTML_STYLESHEET = 120 | HTML_ALIGN_MEMBERS = YES 121 | GENERATE_HTMLHELP = $(GENERATE_CHM) 122 | CHM_FILE = ../$(PROJECT).chm 123 | HHC_LOCATION = $(HHC_PATH) 124 | GENERATE_CHI = $(GENERATE_CHI) 125 | BINARY_TOC = NO 126 | TOC_EXPAND = NO 127 | DISABLE_INDEX = NO 128 | ENUM_VALUES_PER_LINE = 4 129 | GENERATE_TREEVIEW = YES 130 | TREEVIEW_WIDTH = 250 131 | 132 | #--------------------------------------------------------------------------- 133 | # configuration options related to the LaTeX output 134 | #--------------------------------------------------------------------------- 135 | GENERATE_LATEX = $(GENERATE_LATEX) 136 | LATEX_OUTPUT = latex 137 | LATEX_CMD_NAME = latex 138 | MAKEINDEX_CMD_NAME = makeindex 139 | COMPACT_LATEX = NO 140 | PAPER_TYPE = $(PAPER_SIZE) 141 | EXTRA_PACKAGES = 142 | LATEX_HEADER = 143 | PDF_HYPERLINKS = NO 144 | USE_PDFLATEX = NO 145 | LATEX_BATCHMODE = YES 146 | LATEX_HIDE_INDICES = NO 147 | 148 | #--------------------------------------------------------------------------- 149 | # configuration options related to the RTF output 150 | #--------------------------------------------------------------------------- 151 | GENERATE_RTF = $(GENERATE_RTF) 152 | RTF_OUTPUT = rtf 153 | COMPACT_RTF = NO 154 | RTF_HYPERLINKS = NO 155 | RTF_STYLESHEET_FILE = 156 | RTF_EXTENSIONS_FILE = 157 | 158 | #--------------------------------------------------------------------------- 159 | # configuration options related to the man page output 160 | #--------------------------------------------------------------------------- 161 | GENERATE_MAN = $(GENERATE_MAN) 162 | MAN_OUTPUT = man 163 | MAN_EXTENSION = .1 164 | MAN_LINKS = NO 165 | 166 | #--------------------------------------------------------------------------- 167 | # configuration options related to the XML output 168 | #--------------------------------------------------------------------------- 169 | GENERATE_XML = $(GENERATE_XML) 170 | XML_OUTPUT = xml 171 | XML_SCHEMA = 172 | XML_DTD = 173 | XML_PROGRAMLISTING = YES 174 | 175 | #--------------------------------------------------------------------------- 176 | # configuration options for the AutoGen Definitions output 177 | #--------------------------------------------------------------------------- 178 | GENERATE_AUTOGEN_DEF = NO 179 | 180 | #--------------------------------------------------------------------------- 181 | # configuration options related to the Perl module output 182 | #--------------------------------------------------------------------------- 183 | GENERATE_PERLMOD = NO 184 | PERLMOD_LATEX = NO 185 | PERLMOD_PRETTY = YES 186 | PERLMOD_MAKEVAR_PREFIX = 187 | 188 | #--------------------------------------------------------------------------- 189 | # Configuration options related to the preprocessor 190 | #--------------------------------------------------------------------------- 191 | ENABLE_PREPROCESSING = YES 192 | MACRO_EXPANSION = NO 193 | EXPAND_ONLY_PREDEF = NO 194 | SEARCH_INCLUDES = YES 195 | INCLUDE_PATH = 196 | INCLUDE_FILE_PATTERNS = 197 | PREDEFINED = 198 | EXPAND_AS_DEFINED = 199 | SKIP_FUNCTION_MACROS = YES 200 | 201 | #--------------------------------------------------------------------------- 202 | # Configuration::additions related to external references 203 | #--------------------------------------------------------------------------- 204 | TAGFILES = 205 | GENERATE_TAGFILE = $(DOCDIR)/$(PROJECT).tag 206 | ALLEXTERNALS = NO 207 | EXTERNAL_GROUPS = YES 208 | PERL_PATH = $(PERL_PATH) 209 | 210 | #--------------------------------------------------------------------------- 211 | # Configuration options related to the dot tool 212 | #--------------------------------------------------------------------------- 213 | CLASS_DIAGRAMS = YES 214 | HIDE_UNDOC_RELATIONS = YES 215 | HAVE_DOT = $(HAVE_DOT) 216 | CLASS_GRAPH = YES 217 | COLLABORATION_GRAPH = YES 218 | GROUP_GRAPHS = YES 219 | UML_LOOK = NO 220 | TEMPLATE_RELATIONS = NO 221 | INCLUDE_GRAPH = YES 222 | INCLUDED_BY_GRAPH = YES 223 | CALL_GRAPH = NO 224 | CALLER_GRAPH = NO 225 | GRAPHICAL_HIERARCHY = YES 226 | DIRECTORY_GRAPH = YES 227 | DOT_IMAGE_FORMAT = png 228 | DOT_PATH = $(DOT_PATH) 229 | DOTFILE_DIRS = 230 | MAX_DOT_GRAPH_WIDTH = 1024 231 | MAX_DOT_GRAPH_HEIGHT = 1024 232 | MAX_DOT_GRAPH_DEPTH = 0 233 | DOT_TRANSPARENT = NO 234 | DOT_MULTI_TARGETS = NO 235 | GENERATE_LEGEND = YES 236 | DOT_CLEANUP = YES 237 | 238 | #--------------------------------------------------------------------------- 239 | # Configuration::additions related to the search engine 240 | #--------------------------------------------------------------------------- 241 | SEARCHENGINE = NO 242 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickConfig.hh: -------------------------------------------------------------------------------- 1 | /* c++/drivers/base/src/SickConfig.hh. Generated from SickConfig.hh.in by configure. */ 2 | /* c++/drivers/base/src/SickConfig.hh.in. Generated from configure.ac by autoheader. */ 3 | 4 | /* Define if building universal (internal helper macro) */ 5 | /* #undef AC_APPLE_UNIVERSAL_BUILD */ 6 | 7 | /* Define to 1 if you have the header file. */ 8 | #define HAVE_ARPA_INET_H 1 9 | 10 | /* Define to 1 if you have the header file. */ 11 | #define HAVE_DLFCN_H 1 12 | 13 | /* Define to 1 if you have the header file. */ 14 | #define HAVE_FCNTL_H 1 15 | 16 | /* Define to 1 if you have the `gettimeofday' function. */ 17 | #define HAVE_GETTIMEOFDAY 1 18 | 19 | /* Define if you have gnuplot */ 20 | /* #undef HAVE_GNUPLOT */ 21 | 22 | /* Define to 1 if you have the header file. */ 23 | #define HAVE_INTTYPES_H 1 24 | 25 | /* Define if you have linux/serial.h */ 26 | #define HAVE_LINUX_SERIAL_H 1 27 | 28 | /* Define to 1 if you have the header file. */ 29 | #define HAVE_MEMORY_H 1 30 | 31 | /* Define to 1 if you have the `memset' function. */ 32 | #define HAVE_MEMSET 1 33 | 34 | /* Define to 1 if you have the header file. */ 35 | #define HAVE_NETINET_IN_H 1 36 | 37 | /* Define if you have POSIX threads libraries and header files. */ 38 | #define HAVE_PTHREAD 1 39 | 40 | /* Define to 1 if you have the `select' function. */ 41 | #define HAVE_SELECT 1 42 | 43 | /* Define to 1 if you have the `socket' function. */ 44 | #define HAVE_SOCKET 1 45 | 46 | /* Define to 1 if stdbool.h conforms to C99. */ 47 | #define HAVE_STDBOOL_H 1 48 | 49 | /* Define to 1 if you have the header file. */ 50 | #define HAVE_STDINT_H 1 51 | 52 | /* Define to 1 if you have the header file. */ 53 | #define HAVE_STDLIB_H 1 54 | 55 | /* Define to 1 if you have the `strcasecmp' function. */ 56 | #define HAVE_STRCASECMP 1 57 | 58 | /* Define to 1 if you have the header file. */ 59 | #define HAVE_STRINGS_H 1 60 | 61 | /* Define to 1 if you have the header file. */ 62 | #define HAVE_STRING_H 1 63 | 64 | /* Define to 1 if you have the header file. */ 65 | #define HAVE_SYS_IOCTL_H 1 66 | 67 | /* Define to 1 if you have the header file. */ 68 | #define HAVE_SYS_SELECT_H 1 69 | 70 | /* Define to 1 if you have the header file. */ 71 | #define HAVE_SYS_SOCKET_H 1 72 | 73 | /* Define to 1 if you have the header file. */ 74 | #define HAVE_SYS_STAT_H 1 75 | 76 | /* Define to 1 if you have the header file. */ 77 | #define HAVE_SYS_TIME_H 1 78 | 79 | /* Define to 1 if you have the header file. */ 80 | #define HAVE_SYS_TYPES_H 1 81 | 82 | /* Define to 1 if you have the header file. */ 83 | #define HAVE_TERMIOS_H 1 84 | 85 | /* Define to 1 if you have the header file. */ 86 | #define HAVE_UNISTD_H 1 87 | 88 | /* Define to 1 if the system has the type `_Bool'. */ 89 | #define HAVE__BOOL 1 90 | 91 | /* Define to the sub-directory in which libtool stores uninstalled libraries. 92 | */ 93 | #define LT_OBJDIR ".libs/" 94 | 95 | /* Name of package */ 96 | #define PACKAGE "sicktoolbox" 97 | 98 | /* Define to the address where bug reports for this package should be sent. */ 99 | #define PACKAGE_BUGREPORT "jasonder@seas.upenn.edu" 100 | 101 | /* Define to the full name of this package. */ 102 | #define PACKAGE_NAME "SickToolbox" 103 | 104 | /* Define to the full name and version of this package. */ 105 | #define PACKAGE_STRING "SickToolbox 1.1" 106 | 107 | /* Define to the one symbol short name of this package. */ 108 | #define PACKAGE_TARNAME "sicktoolbox" 109 | 110 | /* Define to the home page for this package. */ 111 | #define PACKAGE_URL "" 112 | 113 | /* Define to the version of this package. */ 114 | #define PACKAGE_VERSION "1.1" 115 | 116 | /* Define to necessary symbol if this constant uses a non-standard name on 117 | your system. */ 118 | /* #undef PTHREAD_CREATE_JOINABLE */ 119 | 120 | /* Define as the return type of signal handlers (`int' or `void'). */ 121 | #define RETSIGTYPE void 122 | 123 | /* Define to the type of arg 1 for `select'. */ 124 | #define SELECT_TYPE_ARG1 int 125 | 126 | /* Define to the type of args 2, 3 and 4 for `select'. */ 127 | #define SELECT_TYPE_ARG234 (fd_set *) 128 | 129 | /* Define to the type of arg 5 for `select'. */ 130 | #define SELECT_TYPE_ARG5 (struct timeval *) 131 | 132 | /* Define to 1 if you have the ANSI C header files. */ 133 | #define STDC_HEADERS 1 134 | 135 | /* Define to 1 if you can safely include both and . */ 136 | #define TIME_WITH_SYS_TIME 1 137 | 138 | /* Version number of package */ 139 | #define VERSION "1.1" 140 | 141 | /* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most 142 | significant byte first (like Motorola and SPARC, unlike Intel). */ 143 | #if defined AC_APPLE_UNIVERSAL_BUILD 144 | # if defined __BIG_ENDIAN__ 145 | # define WORDS_BIGENDIAN 1 146 | # endif 147 | #else 148 | # ifndef WORDS_BIGENDIAN 149 | /* # undef WORDS_BIGENDIAN */ 150 | # endif 151 | #endif 152 | 153 | /* Define for Solaris 2.5.1 so the uint32_t typedef from , 154 | , or is not used. If the typedef were allowed, the 155 | #define below would cause a syntax error. */ 156 | /* #undef _UINT32_T */ 157 | 158 | /* Define for Solaris 2.5.1 so the uint8_t typedef from , 159 | , or is not used. If the typedef were allowed, the 160 | #define below would cause a syntax error. */ 161 | /* #undef _UINT8_T */ 162 | 163 | /* Define to empty if `const' does not conform to ANSI C. */ 164 | /* #undef const */ 165 | 166 | /* Define to `__inline__' or `__inline' if that's what the C compiler 167 | calls it, or to nothing if 'inline' is not supported under any name. */ 168 | #ifndef __cplusplus 169 | /* #undef inline */ 170 | #endif 171 | 172 | /* Define to the type of a signed integer type of width exactly 16 bits if 173 | such a type exists and the standard includes do not define it. */ 174 | /* #undef int16_t */ 175 | 176 | /* Define to the type of an unsigned integer type of width exactly 16 bits if 177 | such a type exists and the standard includes do not define it. */ 178 | /* #undef uint16_t */ 179 | 180 | /* Define to the type of an unsigned integer type of width exactly 32 bits if 181 | such a type exists and the standard includes do not define it. */ 182 | /* #undef uint32_t */ 183 | 184 | /* Define to the type of an unsigned integer type of width exactly 8 bits if 185 | such a type exists and the standard includes do not define it. */ 186 | /* #undef uint8_t */ 187 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickException.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickException.hh 3 | * \brief Contains some simple exception classes. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #ifndef SICK_EXCEPTION 17 | #define SICK_EXCEPTION 18 | 19 | /* Definition dependencies */ 20 | #include 21 | #include 22 | 23 | /* Associate the namespace */ 24 | namespace SickToolbox { 25 | 26 | /** \class SickException 27 | * \brief Provides a base exception class from 28 | * which to derive other Sick exceptions 29 | */ 30 | class SickException : std::exception { 31 | 32 | public: 33 | 34 | /** 35 | * \brief A standard constructor 36 | * \param general_str A descriptive "general" string 37 | */ 38 | SickException( const std::string general_str ) { 39 | _detailed_msg = general_str; 40 | } 41 | 42 | /** 43 | * \brief A standard constructor 44 | * \param general_str A descriptive "general" string 45 | * \param detailed_str A more detailed description 46 | */ 47 | SickException( const std::string general_str, const std::string detailed_str ) { 48 | _detailed_msg = general_str + " " + detailed_str; 49 | } 50 | 51 | /** 52 | * \brief From the standard exception library 53 | */ 54 | virtual const char* what( ) const throw() { 55 | return _detailed_msg.c_str(); 56 | } 57 | 58 | /** 59 | * \brief A destructor 60 | */ 61 | ~SickException() throw() {} 62 | 63 | private: 64 | 65 | /** The string identifier */ 66 | std::string _detailed_msg; 67 | 68 | }; 69 | 70 | /** 71 | * \class SickTimeoutException 72 | * \brief Makes handling timeouts much easier 73 | */ 74 | class SickTimeoutException : public SickException { 75 | 76 | public: 77 | 78 | /** 79 | * \brief A constructor 80 | */ 81 | SickTimeoutException() : 82 | SickException("A Timeout Occurred!") { } 83 | 84 | /** 85 | * \brief A constructor 86 | * \param detailed_str A more detailed description 87 | */ 88 | SickTimeoutException( const std::string detailed_str ) : 89 | SickException("A Timeout Occurred -",detailed_str) { } 90 | 91 | /** 92 | * \brief A destructor 93 | */ 94 | ~SickTimeoutException() throw() { } 95 | 96 | }; 97 | 98 | /** 99 | * \class SickIOException 100 | * \brief Thrown instance where the driver can't 101 | * read,write,drain,flush,... the buffers 102 | */ 103 | class SickIOException : public SickException { 104 | 105 | public: 106 | 107 | /** 108 | * \brief A constructor 109 | */ 110 | SickIOException() : 111 | SickException("ERROR: I/O exception!") { } 112 | 113 | /** 114 | * \brief Another constructor 115 | * \param detailed_str A more detailed description 116 | */ 117 | SickIOException( const std::string detailed_str ) : 118 | SickException("ERROR: I/O exception -",detailed_str) { } 119 | 120 | /** 121 | * \brief A destructor 122 | */ 123 | ~SickIOException() throw() { } 124 | 125 | }; 126 | 127 | /** 128 | * \class SickBadChecksumException 129 | * \brief Thrown when a received message has an 130 | * invalid checksum 131 | */ 132 | class SickBadChecksumException : public SickException { 133 | 134 | public: 135 | 136 | /** 137 | * \brief A constructor 138 | */ 139 | SickBadChecksumException() : 140 | SickException("ERROR: Bad Checksum!") { } 141 | 142 | /** 143 | * \brief Another constructor 144 | * \param detailed_str A more detailed description 145 | */ 146 | SickBadChecksumException( const std::string detailed_str ) : 147 | SickException("ERROR: Bad Checksum -",detailed_str) { } 148 | 149 | /** 150 | * \brief A destructor 151 | */ 152 | ~SickBadChecksumException() throw() { } 153 | 154 | }; 155 | 156 | /** 157 | * \class SickThreadException 158 | * \brief Thrown when error occurs during thread 159 | * initialization, and uninitialization 160 | */ 161 | class SickThreadException : public SickException { 162 | 163 | public: 164 | 165 | /** 166 | * \brief A constructor 167 | */ 168 | SickThreadException() : 169 | SickException("ERROR: Sick thread exception!") { } 170 | 171 | /** 172 | * \brief Another constructor 173 | * \param detailed_str A more detailed description 174 | */ 175 | SickThreadException( const std::string detailed_str ) : 176 | SickException("ERROR: Sick thread exception -",detailed_str) { } 177 | 178 | /** 179 | * \brief A destructor 180 | */ 181 | ~SickThreadException() throw() { } 182 | 183 | }; 184 | 185 | /** 186 | * \class SickConfigException 187 | * \brief Thrown when the driver detects (or the Sick reports) 188 | * an invalid config 189 | */ 190 | class SickConfigException : public SickException { 191 | 192 | public: 193 | 194 | /** 195 | * \brief A constructor 196 | */ 197 | SickConfigException() : 198 | SickException("ERROR: Config exception!") { } 199 | 200 | /** 201 | * \brief Another constructor 202 | * \param detailed_str A more detailed description 203 | */ 204 | SickConfigException( const std::string detailed_str ) : 205 | SickException("ERROR: Config exception -",detailed_str) { } 206 | 207 | /** 208 | * \brief A destructor 209 | */ 210 | ~SickConfigException() throw() { } 211 | 212 | }; 213 | 214 | /** 215 | * \class SickErrorException 216 | * \brief Thrown when Sick returns an error code 217 | * or an unexpected response. 218 | */ 219 | class SickErrorException : public SickException { 220 | 221 | public: 222 | 223 | /** 224 | * \brief A constructor 225 | */ 226 | SickErrorException() : 227 | SickException("ERROR: Sick returned error code!") { }; 228 | 229 | /** 230 | * \brief Another constructor 231 | * \param detailed_str A more detailed description 232 | */ 233 | SickErrorException( const std::string detailed_str ) : 234 | SickException("ERROR: Sick error -", detailed_str) { } 235 | 236 | /** 237 | * \brief A destructor 238 | */ 239 | ~SickErrorException() throw() { } 240 | 241 | }; 242 | } /* namespace SickToolbox */ 243 | 244 | #endif /* SICK_EXCEPTION */ 245 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLDBufferMonitor.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLDBufferMonitor.hh 3 | * \brief Defines a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LMS LIDAR. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #ifndef SICK_LD_BUFFER_MONITOR_HH 18 | #define SICK_LD_BUFFER_MONITOR_HH 19 | 20 | #define DEFAULT_SICK_BYTE_TIMEOUT (35000) ///< Max allowable time between consecutive bytes 21 | 22 | /* Definition dependencies */ 23 | #include "SickLDMessage.hh" 24 | #include "SickBufferMonitor.hh" 25 | #include "SickException.hh" 26 | 27 | /* Associate the namespace */ 28 | namespace SickToolbox { 29 | 30 | /*! 31 | * \brief A class for monitoring the receive buffer when interfacing with a Sick LD LIDAR 32 | */ 33 | class SickLDBufferMonitor : public SickBufferMonitor< SickLDBufferMonitor, SickLDMessage > { 34 | 35 | public: 36 | 37 | /** A standard constructor */ 38 | SickLDBufferMonitor( ); 39 | 40 | /** A method for extracting a single message from the stream */ 41 | void GetNextMessageFromDataStream( SickLDMessage &sick_message ) throw( SickIOException ); 42 | 43 | /** A standard destructor */ 44 | ~SickLDBufferMonitor( ); 45 | 46 | }; 47 | 48 | } /* namespace SickToolbox */ 49 | 50 | #endif /* SICK_LD_BUFFER_MONITOR_HH */ 51 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLDMessage.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLDMessage.hh 3 | * \brief Defines the class SickLDMessage. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #ifndef SICK_LD_MESSAGE_HH 17 | #define SICK_LD_MESSAGE_HH 18 | 19 | /* Definition dependencies */ 20 | #include 21 | #include 22 | #include "SickMessage.hh" 23 | 24 | #define SICK_LD_MSG_HEADER_LEN (8) ///< Sick LD message header length in bytes 25 | #define SICK_LD_MSG_PAYLOAD_MAX_LEN (5816) ///< Sick LD maximum payload length 26 | #define SICK_LD_MSG_TRAILER_LEN (1) ///< Sick LD length of the message trailer 27 | 28 | /* Associate the namespace */ 29 | namespace SickToolbox { 30 | 31 | /** 32 | * \class SickLDMessage 33 | * \brief A class to represent all messages sent to and from the Sick LD unit. 34 | */ 35 | class SickLDMessage : public SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN > { 36 | 37 | public: 38 | 39 | /** A standard constructor */ 40 | SickLDMessage( ); 41 | 42 | /** Constructs a packet by using BuildMessage */ 43 | SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ); 44 | 45 | /** Constructs a packet using ParseMessage() */ 46 | SickLDMessage( const uint8_t * const message_buffer ); 47 | 48 | /** Construct a well-formed raw packet */ 49 | void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ); 50 | 51 | /** Populates fields from a (well-formed) raw packet */ 52 | void ParseMessage( const uint8_t * const message_buffer ); 53 | 54 | /** Get the length of the service code associated with the message */ 55 | uint8_t GetServiceCode( ) const { return _message_buffer[8]; } 56 | 57 | /** Get the service sub-code associated with the message */ 58 | uint8_t GetServiceSubcode( ) const { return _message_buffer[9]; } 59 | 60 | /** Get the checksum for the packet */ 61 | uint8_t GetChecksum( ) const { return _message_buffer[_message_length-1]; } 62 | 63 | /** A debugging function that prints the contents of the frame. */ 64 | void Print( ) const; 65 | 66 | /** Destructor */ 67 | ~SickLDMessage( ); 68 | 69 | private: 70 | 71 | /** Computes the checksum of the frame. 72 | * NOTE: Uses XOR of single bytes over packet payload data. 73 | */ 74 | uint8_t _computeXOR( const uint8_t * const data, const uint32_t length ); 75 | 76 | }; 77 | 78 | } /* namespace SickToolbox */ 79 | 80 | #endif /* SICK_LD_MESSAGE_HH */ 81 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLDUtility.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLDUtility.hh 3 | * \brief Defines simple utility functions for working with the Sick LD. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | /* Auto-generated header */ 17 | #include "SickConfig.hh" 18 | 19 | /** 20 | * \def REVERSE_BYTE_ORDER_16 21 | * \brief Reverses the byte order of the given 16 bit unsigned integer 22 | */ 23 | #define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) ) 24 | 25 | /** 26 | * \def REVERSE_BYTE_ORDER_32 27 | * \brief Reverses the byte order of the given 32 bit unsigned integer 28 | */ 29 | #define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) ) 30 | 31 | /* Associate the namespace */ 32 | namespace SickToolbox { 33 | 34 | #ifndef WORDS_BIGENDIAN 35 | 36 | /* NOTE: The following functions are necessary since the Sick LD doesn't adopt the 37 | * convention of converting from network byte order. 38 | */ 39 | 40 | /** 41 | * \brief Converts host byte order (little-endian) to Sick LD byte order (big-endian) 42 | * \param value The 2-byte value to convert to big-endian 43 | * \return Value in Sick LD byte order (big-endian) 44 | */ 45 | inline uint16_t host_to_sick_ld_byte_order( uint16_t value ) { 46 | return REVERSE_BYTE_ORDER_16(value); 47 | } 48 | 49 | /** 50 | * \brief Converts host byte order (little-endian) to Sick LD byte order (big-endian) 51 | * \param value The 4-byte value to convert to big-endian 52 | * \return Value in Sick LD byte order (big-endian) 53 | */ 54 | inline uint32_t host_to_sick_ld_byte_order( uint32_t value ) { 55 | return REVERSE_BYTE_ORDER_32(value); 56 | } 57 | 58 | /** 59 | * \brief Converts Sick LD byte order (big-endian) to host byte order (little-endian) 60 | * \param value The 2-byte value to convert to little-endian 61 | * \return Value in host byte order (little-endian) 62 | */ 63 | inline uint16_t sick_ld_to_host_byte_order( uint16_t value ) { 64 | return REVERSE_BYTE_ORDER_16(value); 65 | } 66 | 67 | /** 68 | * \brief Converts Sick LD byte order (big-endian) to host byte order (little-endian) 69 | * \param value The 4-byte value to convert to little-endian 70 | * \return Value in host byte order (little-endian) 71 | */ 72 | inline uint32_t sick_ld_to_host_byte_order( uint32_t value ) { 73 | return REVERSE_BYTE_ORDER_32(value); 74 | } 75 | 76 | #else // The host has a big-endian architecture 77 | 78 | /** 79 | * \brief Converts host byte order (big-endian) to Sick LD byte order (big-endian) 80 | * \param value The 2-byte value to convert to big-endian 81 | * \return Value in Sick LD byte order (big-endian) 82 | */ 83 | inline uint16_t host_to_sick_ld_byte_order( uint16_t value ) { 84 | return value; 85 | } 86 | 87 | /** 88 | * \brief Converts host byte order (big-endian) to Sick LD byte order (big-endian) 89 | * \param value The 4-byte value to convert to big-endian 90 | * \return Value in Sick LD byte order (big-endian) 91 | */ 92 | inline uint32_t host_to_sick_ld_byte_order( uint32_t value ) { 93 | return value; 94 | } 95 | 96 | /** 97 | * \brief Converts Sick LD byte order (big-endian) to host byte order (big-endian) 98 | * \param value The 2-byte value to convert to big-endian 99 | * \return Value in host byte order (big-endian) 100 | */ 101 | inline uint16_t sick_ld_to_host_byte_order( uint16_t value ) { 102 | return value; 103 | } 104 | 105 | /** 106 | * \brief Converts Sick LD byte order (big-endian) to host byte order (big-endian) 107 | * \param value The 4-byte value to convert to big-endian 108 | * \return Value in host byte order (big-endian) 109 | */ 110 | inline uint32_t sick_ld_to_host_byte_order( uint32_t value ) { 111 | return value; 112 | } 113 | 114 | #endif /* _LITTLE_ENDIAN_HOST */ 115 | 116 | /* 117 | * NOTE: Other utility functions can be defined here 118 | */ 119 | 120 | } //namespace SickToolbox 121 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLIDAR.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLIDAR.hh 3 | * \brief Defines the abstract parent class for defining 4 | * a Sick LIDAR device driver. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /** 18 | * \mainpage The Sick LIDAR Matlab/C++ Toolbox 19 | * \author Jason C. Derenick and Thomas H. Miller. 20 | * \version 1.0 21 | * 22 | * The Sick LIDAR Matlab/C++ Toolbox 23 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 24 | * All rights reserved. 25 | * 26 | * This software is released under a BSD Open-Source License. 27 | * See http://sicktoolbox.sourceforge.net 28 | */ 29 | 30 | #ifndef SICK_LIDAR 31 | #define SICK_LIDAR 32 | 33 | /* Definition dependencies */ 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include "SickException.hh" 44 | 45 | /* Associate the namespace */ 46 | namespace SickToolbox { 47 | 48 | /** 49 | * \class SickLIDAR 50 | * \brief Provides an abstract parent for all Sick LIDAR devices 51 | */ 52 | template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 53 | class SickLIDAR { 54 | 55 | public: 56 | 57 | /** The primary constructor */ 58 | SickLIDAR( ); 59 | 60 | /** Indicates whether device is initialized */ 61 | bool IsInitialized() { return _sick_initialized; } 62 | 63 | /** A virtual destructor */ 64 | virtual ~SickLIDAR( ); 65 | 66 | protected: 67 | 68 | /** Sick device file descriptor */ 69 | int _sick_fd; 70 | 71 | /** A flag to indicated whether the device is properly initialized */ 72 | bool _sick_initialized; 73 | 74 | /** A pointer to the driver's buffer monitor */ 75 | SICK_MONITOR_CLASS *_sick_buffer_monitor; 76 | 77 | /** Indicates whether the Sick buffer monitor is running */ 78 | bool _sick_monitor_running; 79 | 80 | /** A method for setting up a general connection */ 81 | virtual void _setupConnection( ) = 0; 82 | 83 | /** A method for tearing down a connection to the Sick */ 84 | virtual void _teardownConnection( ) = 0; 85 | 86 | /** Starts the driver listening for messages */ 87 | void _startListening( ) throw( SickThreadException ); 88 | 89 | /** Stops the driver from listening */ 90 | void _stopListening( ) throw( SickThreadException ); 91 | 92 | /** Indicates whether there is a monitor currently running */ 93 | bool _monitorRunning( ) const { return _sick_monitor_running; } 94 | 95 | /** Make the associated file descriptor non blocking */ 96 | void _setBlockingIO( ) const throw ( SickIOException ); 97 | 98 | /** Make the associated file descriptor non blocking */ 99 | void _setNonBlockingIO( ) const throw ( SickIOException ); 100 | 101 | /** Send a message to the Sick LD (allows specifying min time between transmitted bytes) */ 102 | void _sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const 103 | throw( SickIOException ); 104 | 105 | /** Acquire the next message from the message container */ 106 | void _recvMessage( SICK_MSG_CLASS &sick_message, const unsigned int timeout_value ) const throw ( SickTimeoutException ); 107 | 108 | /** Search the stream for a payload with a particular "header" byte string */ 109 | void _recvMessage( SICK_MSG_CLASS &sick_message, 110 | const uint8_t * const byte_sequence, 111 | const unsigned int byte_sequence_length, 112 | const unsigned int timeout_value ) const throw ( SickTimeoutException ); 113 | 114 | /** An inline function for computing elapsed time */ 115 | double _computeElapsedTime( const struct timeval &beg_time, const struct timeval &end_time ) const { return ((end_time.tv_sec*1e6)+(end_time.tv_usec))-((beg_time.tv_sec*1e6)+beg_time.tv_usec); } 116 | 117 | /** Sends a request to the Sick and acquires looks for the reply */ 118 | virtual void _sendMessageAndGetReply( const SICK_MSG_CLASS &send_message, 119 | SICK_MSG_CLASS &recv_message, 120 | const uint8_t * const byte_sequence, 121 | const unsigned int byte_sequence_length, 122 | const unsigned int byte_interval, 123 | const unsigned int timeout_value, 124 | const unsigned int num_tries ) throw( SickTimeoutException, SickIOException); 125 | 126 | }; 127 | 128 | /** 129 | * \brief Initializes the buffer monitor 130 | */ 131 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 132 | SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickLIDAR( ) : 133 | _sick_fd(0), _sick_initialized(false), _sick_buffer_monitor(NULL), _sick_monitor_running(false) { 134 | 135 | try { 136 | /* Attempt to instantiate a new SickBufferMonitor for the device */ 137 | _sick_buffer_monitor = new SICK_MONITOR_CLASS; 138 | } 139 | catch ( std::bad_alloc &allocation_exception ) { 140 | std::cerr << "SickLIDAR::SickLIDAR: Allocation error - " << allocation_exception.what() << std::endl; 141 | } 142 | 143 | } 144 | 145 | /** 146 | * \brief Destructor tears down buffer monitor 147 | */ 148 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 149 | SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickLIDAR( ) { 150 | 151 | /* Deallocate the buffer monitor */ 152 | if (_sick_buffer_monitor) { 153 | delete _sick_buffer_monitor; 154 | } 155 | 156 | } 157 | 158 | /** 159 | * \brief Activates the buffer monitor for the driver 160 | */ 161 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 162 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_startListening( ) throw( SickThreadException ) { 163 | 164 | /* Try to start the monitor */ 165 | try { 166 | _sick_buffer_monitor->StartMonitor(_sick_fd); 167 | } 168 | 169 | /* Handle a thread exception */ 170 | catch(SickThreadException &sick_thread_exception) { 171 | std::cerr << sick_thread_exception.what() << std::endl; 172 | throw; 173 | } 174 | 175 | /* Handle a thread exception */ 176 | catch(...) { 177 | std::cerr << "SickLIDAR::_startListening: Unknown exception!!!" << std::endl; 178 | throw; 179 | } 180 | 181 | /* Set the flag */ 182 | _sick_monitor_running = true; 183 | 184 | } 185 | 186 | /** 187 | * \brief Activates the buffer monitor for the driver 188 | */ 189 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 190 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_stopListening( ) throw( SickThreadException ) { 191 | 192 | /* Try to start the monitor */ 193 | try { 194 | _sick_buffer_monitor->StopMonitor(); 195 | } 196 | 197 | /* Handle a thread exception */ 198 | catch(SickThreadException &sick_thread_exception) { 199 | std::cerr << sick_thread_exception.what() << std::endl; 200 | throw; 201 | } 202 | 203 | /* Handle a thread exception */ 204 | catch(...) { 205 | std::cerr << "SickLIDAR::_stopListening: Unknown exception!!!" << std::endl; 206 | throw; 207 | } 208 | 209 | /* Reset the flag */ 210 | _sick_monitor_running = false; 211 | 212 | } 213 | 214 | /** 215 | * \brief A simple method for setting blocking I/O 216 | */ 217 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 218 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setBlockingIO( ) const throw( SickIOException ) { 219 | 220 | /* Read the flags */ 221 | int fd_flags = 0; 222 | if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) { 223 | throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!"); 224 | } 225 | 226 | /* Set the new flags */ 227 | if(fcntl(_sick_fd,F_SETFL,fd_flags & (~O_NONBLOCK)) < 0) { 228 | throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!"); 229 | } 230 | 231 | } 232 | 233 | /** 234 | * \brief A simple method for setting non-blocking I/O 235 | */ 236 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 237 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setNonBlockingIO( ) const throw ( SickIOException ) { 238 | 239 | /* Read the flags */ 240 | int fd_flags = 0; 241 | if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) { 242 | throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!"); 243 | } 244 | 245 | /* Set the new flags */ 246 | if(fcntl(_sick_fd,F_SETFL,fd_flags | O_NONBLOCK) < 0) { 247 | throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!"); 248 | } 249 | 250 | } 251 | 252 | /** 253 | * \brief Sends a message to the Sick device 254 | * \param &sick_message A reference to the well-formed message that is to be sent to the Sick 255 | * \param byte_interval Minimum time in microseconds between transmitted bytes 256 | */ 257 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 258 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const 259 | throw( SickIOException ) { 260 | 261 | uint8_t message_buffer[SICK_MSG_CLASS::MESSAGE_MAX_LENGTH] = {0}; 262 | 263 | /* Copy the given message and get the message length */ 264 | sick_message.GetMessage(message_buffer); 265 | unsigned int message_length = sick_message.GetMessageLength(); 266 | 267 | /* Check whether a transmission delay between bytes is requested */ 268 | if (byte_interval == 0) { 269 | 270 | /* Write the message to the stream */ 271 | if ((unsigned int)write(_sick_fd,message_buffer,message_length) != message_length) { 272 | throw SickIOException("SickLIDAR::_sendMessage: write() failed!"); 273 | } 274 | 275 | } 276 | else { 277 | 278 | /* Write the message to the unit one byte at a time */ 279 | for (unsigned int i = 0; i < message_length; i++) { 280 | 281 | /* Write a single byte to the stream */ 282 | if (write(_sick_fd,&message_buffer[i],1) != 1) { 283 | throw SickIOException("SickLIDAR::_sendMessage: write() failed!"); 284 | } 285 | 286 | /* Some time between bytes (Sick LMS 2xx likes this) */ 287 | usleep(byte_interval); 288 | } 289 | 290 | } 291 | 292 | } 293 | 294 | /** 295 | * \brief Attempt to acquire the latest available message from the device 296 | * \param &sick_message A reference to the container that will hold the most recent message 297 | * \param timeout_value The time in secs to wait before throwing a timeout error 298 | * \return True if a new message was received, False otherwise 299 | */ 300 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 301 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message, 302 | const unsigned int timeout_value ) const throw ( SickTimeoutException ) { 303 | 304 | /* Timeval structs for handling timeouts */ 305 | struct timeval beg_time, end_time; 306 | 307 | /* Acquire the elapsed time since epoch */ 308 | gettimeofday(&beg_time,NULL); 309 | 310 | /* Check the shared object */ 311 | while(!_sick_buffer_monitor->GetNextMessageFromMonitor(sick_message)) { 312 | 313 | /* Sleep a little bit */ 314 | usleep(1000); 315 | 316 | /* Check whether the allowed time has expired */ 317 | gettimeofday(&end_time,NULL); 318 | if (_computeElapsedTime(beg_time,end_time) > timeout_value) { 319 | throw SickTimeoutException("SickLIDAR::_recvMessage: Timeout occurred!"); 320 | } 321 | 322 | } 323 | 324 | } 325 | 326 | /** 327 | * \brief Attempt to acquire a message having a payload beginning w/ the given byte sequence 328 | * \param &sick_message A reference to the container that will hold the most recent message 329 | * \param *byte_sequence The byte sequence that is expected to lead off the payload in the packet (e.g. service codes, etc...) 330 | * \param byte_sequence_length The number of bytes in the given byte_sequence 331 | * \param timeout_value The time in usecs to wait before throwing a timeout error 332 | * \return True if a new message was received, False otherwise 333 | * 334 | * NOTE: This method is intended to be a helper for _sendMessageAndGetReply 335 | */ 336 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 337 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message, 338 | const uint8_t * const byte_sequence, 339 | const unsigned int byte_sequence_length, 340 | const unsigned int timeout_value ) const throw( SickTimeoutException ) { 341 | 342 | /* Define a buffer */ 343 | uint8_t payload_buffer[SICK_MSG_CLASS::MESSAGE_PAYLOAD_MAX_LENGTH]; 344 | 345 | /* Timeval structs for handling timeouts */ 346 | struct timeval beg_time, end_time; 347 | 348 | /* A container for the message */ 349 | SICK_MSG_CLASS curr_message; 350 | 351 | /* Get the elapsed time since epoch */ 352 | gettimeofday(&beg_time,NULL); 353 | 354 | /* Check until it is found or a timeout */ 355 | for(;;) { 356 | 357 | /* Attempt to acquire the message */ 358 | unsigned int i = 0; 359 | if (_sick_buffer_monitor->GetNextMessageFromMonitor(curr_message)) { 360 | 361 | /* Extract the payload subregion */ 362 | curr_message.GetPayloadSubregion(payload_buffer,0,byte_sequence_length-1); 363 | 364 | /* Match the byte sequence */ 365 | for (i=0; (i < byte_sequence_length) && (payload_buffer[i] == byte_sequence[i]); i++); 366 | 367 | /* Our message was found! */ 368 | if (i == byte_sequence_length) { 369 | sick_message = curr_message; 370 | break; 371 | } 372 | 373 | } 374 | 375 | /* Sleep a little bit */ 376 | usleep(1000); 377 | 378 | /* Check whether the allowed time has expired */ 379 | gettimeofday(&end_time,NULL); 380 | if (_computeElapsedTime(beg_time,end_time) > timeout_value) { 381 | throw SickTimeoutException(); 382 | } 383 | 384 | } 385 | 386 | } 387 | 388 | /** 389 | * \param sick_send_frame A sick frame to be sent to the LMS 390 | * \param sick_receive_frame A sick frame to hold the response (expected or unexpected) of the LMS 391 | * \param num_tries The number of times to send the frame in the event the LMS fails to reply 392 | * \param timeout The epoch to wait before considering a sent frame lost 393 | * \return True if the message was sent and the expected reply was received 394 | */ 395 | template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS > 396 | void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessageAndGetReply( const SICK_MSG_CLASS &send_message, 397 | SICK_MSG_CLASS &recv_message, 398 | const uint8_t * const byte_sequence, 399 | const unsigned int byte_sequence_length, 400 | const unsigned int byte_interval, 401 | const unsigned int timeout_value, 402 | const unsigned int num_tries ) 403 | throw( SickTimeoutException, SickIOException ) { 404 | 405 | /* Send the message for at most num_tries number of times */ 406 | for(unsigned int i = 0; i < num_tries; i++) { 407 | 408 | try { 409 | 410 | /* Send the frame to the unit */ 411 | _sendMessage(send_message,byte_interval); 412 | 413 | /* Wait for the reply! */ 414 | _recvMessage(recv_message,byte_sequence,byte_sequence_length,timeout_value); 415 | 416 | /* message was found! */ 417 | break; 418 | 419 | } 420 | 421 | /* Handle a timeout! */ 422 | catch (SickTimeoutException &sick_timeout) { 423 | 424 | /* Check if it was found! */ 425 | if (i == num_tries - 1) { 426 | throw SickTimeoutException("SickLIDAR::_sendMessageAndGetReply: Attempted max number of tries w/o success!"); 427 | } 428 | 429 | /* Display the number of tries remaining! */ 430 | std::cerr << sick_timeout.what() << " " << num_tries - i - 1 << " tries remaining" << std::endl; 431 | 432 | } 433 | 434 | /* Handle write buffer exceptions */ 435 | catch (SickIOException &sick_io_error) { 436 | std::cerr << sick_io_error.what() << std::endl; 437 | throw; 438 | } 439 | 440 | /* A safety net */ 441 | catch (...) { 442 | std::cerr << "SickLIDAR::_sendMessageAndGetReply: Unknown exception!!!" << std::endl; 443 | throw; 444 | } 445 | 446 | } 447 | 448 | } 449 | 450 | } /* namespace SickToolbox */ 451 | 452 | #endif /* SICK_LIDAR */ 453 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS1xx.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xx.hh 3 | * \brief Defines the SickLMS1xx class for working with the 4 | * Sick LMS1xx laser range finders. 5 | * 6 | * Code by Jason C. Derenick and Christopher R. Mansley. 7 | * Contact jasonder(at)seas(dot)upenn(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | #ifndef SICK_LMS_1XX_HH 18 | #define SICK_LMS_1XX_HH 19 | 20 | /* Macros */ 21 | #define DEFAULT_SICK_LMS_1XX_IP_ADDRESS "192.168.0.1" ///< Default IP Address 22 | #define DEFAULT_SICK_LMS_1XX_TCP_PORT (2111) ///< Sick LMS 1xx TCP/IP Port 23 | #define DEFAULT_SICK_LMS_1XX_CONNECT_TIMEOUT (1000000) ///< Max time for establishing connection (usecs) 24 | #define DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT (5000000) ///< Max time for reply (usecs) 25 | #define DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT (60000000) ///< Max time it should take to change status 26 | 27 | #define SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE (-450000) ///< -45 degrees (1/10000) degree 28 | #define SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE (2250000) ///< 225 degrees (1/10000) degree 29 | 30 | /* Definition dependencies */ 31 | #include 32 | #include 33 | 34 | #include "SickLIDAR.hh" 35 | #include "SickLMS1xxBufferMonitor.hh" 36 | #include "SickLMS1xxMessage.hh" 37 | #include "SickException.hh" 38 | 39 | /** 40 | * \namespace SickToolbox 41 | * \brief Encapsulates the Sick LIDAR Matlab/C++ toolbox 42 | */ 43 | namespace SickToolbox { 44 | 45 | /** 46 | * \class SickLMS1xx 47 | * \brief Provides a simple driver interface for working with the 48 | * Sick LD-OEM/Sick LD-LRS long-range models via Ethernet. 49 | */ 50 | class SickLMS1xx : public SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage > { 51 | 52 | public: 53 | 54 | static const int SICK_LMS_1XX_MAX_NUM_MEASUREMENTS = 1082; ///< LMS 1xx max number of measurements 55 | 56 | /*! 57 | * \enum sick_lms_1xx_status_t 58 | * \brief Defines the Sick LMS 1xx status. 59 | * This enum lists all of the Sick LMS 1xx status. 60 | */ 61 | enum sick_lms_1xx_status_t { 62 | 63 | SICK_LMS_1XX_STATUS_UNKNOWN = 0x00, ///< LMS 1xx status undefined 64 | SICK_LMS_1XX_STATUS_INITIALIZATION = 0x01, ///< LMS 1xx initializing 65 | SICK_LMS_1XX_STATUS_CONFIGURATION = 0x02, ///< LMS 1xx configuration 66 | SICK_LMS_1XX_STATUS_IDLE = 0x03, ///< LMS 1xx is idle 67 | SICK_LMS_1XX_STATUS_ROTATED = 0x04, ///< LMS 1xx mirror rotating 68 | SICK_LMS_1XX_STATUS_IN_PREP = 0x05, ///< LMS 1xx in preparation 69 | SICK_LMS_1XX_STATUS_READY = 0x06, ///< LMS 1xx is ready 70 | SICK_LMS_1XX_STATUS_READY_FOR_MEASUREMENT = 0x07 ///< LMS 1xx is ready to give measurements 71 | 72 | }; 73 | 74 | /*! 75 | * \enum sick_lms_1xx_scan_format_t 76 | * \brief Defines the Sick LMS 1xx scan types 77 | * This enum is for specifiying the desired scan returns. 78 | */ 79 | enum sick_lms_1xx_scan_format_t { 80 | 81 | SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE = 0x00, ///< Single-pulse dist, no reflect 82 | SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_8BIT = 0x01, ///< Single-pulse dist, 8bit reflect 83 | SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT = 0x02, ///< Single-pulse dist, 16bit reflect 84 | SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_NONE = 0x03, ///< Double-pulse dist, no reflect 85 | SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_8BIT = 0x04, ///< Double-pulse dist, 8bit reflect 86 | SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT = 0x05, ///< Double-pulse dist, 16bit reflect 87 | SICK_LMS_1XX_SCAN_FORMAT_UNKNOWN = 0xFF ///< Unknown format 88 | 89 | }; 90 | 91 | /*! 92 | * \enum sick_lms_1xx_scan_freq_t 93 | * \brief Defines the Sick LMS 1xx scan frequency. 94 | * This enum lists all of the Sick LMS 1xx scan frequencies. 95 | */ 96 | enum sick_lms_1xx_scan_freq_t { 97 | 98 | SICK_LMS_1XX_SCAN_FREQ_UNKNOWN = 0x00, ///< LMS 1xx scan freq unknown 99 | SICK_LMS_1XX_SCAN_FREQ_25 = 0x09C4, ///< LMS 1xx scan freq 25Hz 100 | SICK_LMS_1XX_SCAN_FREQ_50 = 0X1388 ///< LMS 1xx scan freq 50Hz 101 | 102 | }; 103 | 104 | /*! 105 | * \enum sick_lms_1xx_scan_res_t 106 | * \brief Defines the Sick LMS 1xx scan resolution. 107 | * This enum lists all of the Sick LMS 1xx scan resolutions. 108 | */ 109 | enum sick_lms_1xx_scan_res_t { 110 | 111 | SICK_LMS_1XX_SCAN_RES_UNKNOWN = 0x00, ///< LMS 1xx scab res unknown 112 | SICK_LMS_1XX_SCAN_RES_25 = 0x09C4, ///< LMS 1xx scan res 0.25 deg 113 | SICK_LMS_1XX_SCAN_RES_50 = 0x1388 ///< LMS 1xx scan res 0.50 deg 114 | 115 | }; 116 | 117 | /** Primary constructor */ 118 | SickLMS1xx( const std::string sick_ip_address = DEFAULT_SICK_LMS_1XX_IP_ADDRESS, 119 | const uint16_t sick_tcp_port = DEFAULT_SICK_LMS_1XX_TCP_PORT ); 120 | 121 | /** Initializes the Sick LD unit (use scan areas defined in flash) */ 122 | void Initialize( const bool disp_banner = true ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ); 123 | 124 | /** Sets the Sick LMS 1xx scan frequency and scan resolution */ 125 | void SetSickScanFreqAndRes( const sick_lms_1xx_scan_freq_t scan_freq, 126 | const sick_lms_1xx_scan_res_t scan_res ) throw( SickTimeoutException, SickIOException, SickErrorException ); 127 | 128 | /** Get the Sick LMS 1xx scan frequency */ 129 | sick_lms_1xx_scan_freq_t GetSickScanFreq( ) const throw ( SickIOException ); 130 | 131 | /** Get the Sick LMS 1xx scan resolution */ 132 | sick_lms_1xx_scan_res_t GetSickScanRes( ) const throw ( SickIOException ); 133 | 134 | /** Get the Sick LMS 1xx scan start angle */ 135 | double GetSickStartAngle( ) const throw ( SickIOException ); 136 | 137 | /** Get the Sick LMS 1xx scan stop angle */ 138 | double GetSickStopAngle( ) const throw ( SickIOException ); 139 | 140 | /** Sets the sick scan data format */ 141 | void SetSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException ); 142 | 143 | /** Get the Sick Range Measurements */ 144 | void GetSickMeasurements( unsigned int * const range_1_vals, 145 | unsigned int * const range_2_vals, 146 | unsigned int * const reflect_1_vals, 147 | unsigned int * const reflect_2_vals, 148 | unsigned int & num_measurements, 149 | unsigned int * const dev_status = NULL ) throw ( SickIOException, SickConfigException, SickTimeoutException ); 150 | 151 | /** Uninitializes the Sick LD unit */ 152 | void Uninitialize( const bool disp_banner = true ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException ); 153 | 154 | /** Utility function for converting integer to scan frequency */ 155 | sick_lms_1xx_scan_freq_t IntToSickScanFreq( const int scan_freq ) const; 156 | 157 | /** Utility function for converting scan frequency to integer */ 158 | int SickScanFreqToInt( const sick_lms_1xx_scan_freq_t scan_freq ) const; 159 | 160 | /** Utility function for converting double to scan resolution */ 161 | sick_lms_1xx_scan_res_t DoubleToSickScanRes( const double scan_res ) const; 162 | 163 | /** Utility function for converting scan resolution to double */ 164 | double SickScanResToDouble( const sick_lms_1xx_scan_res_t scan_res ) const; 165 | 166 | /** Destructor */ 167 | ~SickLMS1xx(); 168 | 169 | private: 170 | 171 | /*! 172 | * \struct sick_lms_1xx_scan_config_tag 173 | * \brief A structure for aggregrating the 174 | * Sick LMS 1xx configuration params. 175 | */ 176 | /*! 177 | * \typedef sick_lms_1xx_scan_config_t 178 | * \brief Adopt c-style convention 179 | */ 180 | typedef struct sick_lms_1xx_scan_config_tag { 181 | sick_lms_1xx_scan_freq_t sick_scan_freq; ///< Sick scan frequency 182 | sick_lms_1xx_scan_res_t sick_scan_res; ///< Sick scan resolution 183 | int32_t sick_start_angle; ///< Sick scan area start angle 184 | int32_t sick_stop_angle; ///< Sick scan area stop angle 185 | } sick_lms_1xx_scan_config_t; 186 | 187 | /** The Sick LMS 1xx IP address */ 188 | std::string _sick_ip_address; 189 | 190 | /** The Sick LMS 1xx TCP port number */ 191 | uint16_t _sick_tcp_port; 192 | 193 | /** Sick LMS 1xx socket address struct */ 194 | struct sockaddr_in _sick_inet_address_info; 195 | 196 | /** Sick LMS 1xx configuration struct */ 197 | sick_lms_1xx_scan_config_t _sick_scan_config; 198 | 199 | /** Sick LMS 1xx current scan data format */ 200 | sick_lms_1xx_scan_format_t _sick_scan_format; 201 | 202 | /** Sick LMS 1xx configuration struct */ 203 | sick_lms_1xx_status_t _sick_device_status; 204 | 205 | /** Sick LMS 1xx temperature status */ 206 | bool _sick_temp_safe; 207 | 208 | /** Sick LMS 1xx streaming status */ 209 | bool _sick_streaming; 210 | 211 | /** Setup the connection parameters and establish TCP connection! */ 212 | void _setupConnection( ) throw( SickIOException, SickTimeoutException ); 213 | 214 | /** Re-initialized the device */ 215 | void _reinitialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException ); 216 | 217 | /** Teardown the connection to the Sick LD */ 218 | void _teardownConnection( ) throw( SickIOException ); 219 | 220 | /** Acquire the latest Sick LMS's status */ 221 | void _updateSickStatus( ) throw( SickTimeoutException, SickIOException ); 222 | 223 | /** Acquire the Sick LMS's scan config */ 224 | void _getSickScanConfig( ) throw( SickTimeoutException, SickIOException ); 225 | 226 | /** Sets the scan configuration (volatile, does not write to EEPROM) */ 227 | void _setSickScanConfig( const sick_lms_1xx_scan_freq_t scan_freq, 228 | const sick_lms_1xx_scan_res_t scan_res, 229 | const int start_angle, const int stop_angle ) throw( SickTimeoutException, SickIOException, SickErrorException ); 230 | 231 | /** Set access mode for configuring device */ 232 | void _setAuthorizedClientAccessMode( ) throw( SickTimeoutException, SickErrorException, SickIOException ); 233 | 234 | /** Save configuration parameters to EEPROM */ 235 | void _writeToEEPROM( ) throw( SickTimeoutException, SickIOException ); 236 | 237 | /** Send the message w/o waiting for a reply */ 238 | void _sendMessage( const SickLMS1xxMessage &send_message ) const throw ( SickIOException ); 239 | 240 | /** Send the message and grab expected reply */ 241 | void _sendMessageAndGetReply( const SickLMS1xxMessage &send_message, 242 | SickLMS1xxMessage &recv_message, 243 | const std::string reply_command_code, 244 | const std::string reply_command, 245 | const unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT, 246 | const unsigned int num_tries = 1 ) throw( SickIOException, SickTimeoutException ); 247 | 248 | /** Receive a message */ 249 | void _recvMessage( SickLMS1xxMessage &sick_message ) const throw ( SickTimeoutException ); 250 | 251 | /** Start device measuring */ 252 | void _startMeasuring( ) throw ( SickTimeoutException, SickIOException ); 253 | 254 | /** Stop device measuring */ 255 | void _stopMeasuring( ) throw ( SickTimeoutException, SickIOException ); 256 | 257 | /** Request a data data stream type */ 258 | void _requestDataStream( ) throw ( SickTimeoutException, SickConfigException, SickIOException ); 259 | 260 | /** Start streaming measurements */ 261 | void _startStreamingMeasurements( )throw( SickTimeoutException, SickIOException ); 262 | 263 | /** Stop streaming measurements */ 264 | void _stopStreamingMeasurements( const bool disp_banner = true ) throw( SickTimeoutException, SickIOException ); 265 | 266 | /** Set device to measuring mode */ 267 | void _checkForMeasuringStatus( unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT ) throw( SickTimeoutException, SickIOException ); 268 | 269 | /** Sets the sick scan data format */ 270 | void _setSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException ); 271 | 272 | /** Restore device to measuring mode */ 273 | void _restoreMeasuringMode( ) throw( SickTimeoutException, SickIOException ); 274 | 275 | /** Ensures a feasible scan area */ 276 | bool _validScanArea( const int start_angle, const int stop_angle ) const; 277 | 278 | /** Utility function to convert int to status */ 279 | sick_lms_1xx_status_t _intToSickStatus( const int status ) const; 280 | 281 | /** Utility function for printing Sick scan config */ 282 | void _printSickScanConfig( ) const; 283 | 284 | /** Utility function for printing footer after initialization */ 285 | void _printInitFooter( ) const; 286 | 287 | /** Utility function for returning scan format as string */ 288 | std::string _sickScanDataFormatToString( const sick_lms_1xx_scan_format_t scan_format ) const; 289 | 290 | /** Utility function for converting sick freq to doubles */ 291 | double _convertSickAngleUnitsToDegs( const int sick_angle ) const { return ((double)sick_angle)/10000; } 292 | 293 | /** Utility function for converting sick Hz values ints */ 294 | unsigned int _convertSickFreqUnitsToHz( const unsigned int sick_freq ) const { return (unsigned int)(((double)sick_freq)/100); } 295 | 296 | /** Utility function to convert config error int to str */ 297 | std::string _intToSickConfigErrorStr( const int error ) const; 298 | 299 | /** Utility function to locate substring in string */ 300 | bool _findSubString( const char * const str, const char * const substr, const unsigned int str_length, const unsigned int substr_length, 301 | unsigned int &substr_pos, unsigned int start_pos = 0 ) const; 302 | 303 | /** Utility function for extracting next integer from tokenized string */ 304 | char * _convertNextTokenToUInt( char * const str_buffer, unsigned int & num_val, const char * const delimeter = " " ) const; 305 | 306 | }; 307 | 308 | /*! 309 | * \typedef sick_lms_1xx_status_t 310 | * \brief Makes working w/ SickLMS1xx::sick_lms_1xx_status_t a bit easier 311 | */ 312 | typedef SickLMS1xx::sick_lms_1xx_status_t sick_lms_1xx_status_t; 313 | 314 | /*! 315 | * \typedef sick_lms_1xx_scan_format_t 316 | * \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_format_t a bit easier 317 | */ 318 | typedef SickLMS1xx::sick_lms_1xx_scan_format_t sick_lms_1xx_scan_format_t; 319 | 320 | /*! 321 | * \typedef sick_lms_1xx_scan_freq_t 322 | * \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_freq_t a bit easier 323 | */ 324 | typedef SickLMS1xx::sick_lms_1xx_scan_freq_t sick_lms_1xx_scan_freq_t; 325 | 326 | /*! 327 | * \typedef sick_lms_1xx_scan_res_t 328 | * \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_res_t a bit easier 329 | */ 330 | typedef SickLMS1xx::sick_lms_1xx_scan_res_t sick_lms_1xx_scan_res_t; 331 | 332 | 333 | } //namespace SickToolbox 334 | 335 | #endif /* SICK_LMS_1XX_HH */ 336 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS1xxBufferMonitor.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xxBufferMonitor.hh 3 | * \brief Defines a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LMS 1xx 5 | * laser range finder unit. 6 | * 7 | * Code by Jason C. Derenick and Christopher R. Mansley. 8 | * Contact jasonder(at)seas(dot)upenn(dot)edu 9 | * 10 | * The Sick LIDAR Matlab/C++ Toolbox 11 | * Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley 12 | * All rights reserved. 13 | * 14 | * This software is released under a BSD Open-Source License. 15 | * See http://sicktoolbox.sourceforge.net 16 | */ 17 | 18 | #ifndef SICK_LMS_1XX_BUFFER_MONITOR_HH 19 | #define SICK_LMS_1XX_BUFFER_MONITOR_HH 20 | 21 | #define DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT (100000) ///< Max allowable time between consecutive bytes 22 | 23 | /* Definition dependencies */ 24 | #include "SickLMS1xxMessage.hh" 25 | #include "SickBufferMonitor.hh" 26 | #include "SickException.hh" 27 | 28 | /* Associate the namespace */ 29 | namespace SickToolbox { 30 | 31 | /*! 32 | * \brief A class for monitoring the receive buffer when interfacing with a Sick LD LIDAR 33 | */ 34 | class SickLMS1xxBufferMonitor : public SickBufferMonitor< SickLMS1xxBufferMonitor, SickLMS1xxMessage > { 35 | 36 | public: 37 | 38 | /** A standard constructor */ 39 | SickLMS1xxBufferMonitor( ); 40 | 41 | /** A method for extracting a single message from the stream */ 42 | void GetNextMessageFromDataStream( SickLMS1xxMessage &sick_message ) throw( SickIOException ); 43 | 44 | /** A standard destructor */ 45 | ~SickLMS1xxBufferMonitor( ); 46 | 47 | private: 48 | 49 | /* A utility function for flushing the receive buffer */ 50 | void _flushTCPRecvBuffer( ) const throw ( SickIOException ); 51 | 52 | }; 53 | 54 | } /* namespace SickToolbox */ 55 | 56 | #endif /* SICK_LMS_1XX_BUFFER_MONITOR_HH */ 57 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS1xxMessage.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xx Message.hh 3 | * \brief Defines the class SickLMS1xxMessage. 4 | * 5 | * Code by Jason C. Derenick and Christopher R. Mansley. 6 | * Contact jasonder(at)seas(dot)upenn(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2009, Jason C. Derenick and Christoper R. Mansley 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #ifndef SICK_LMS_1XX_MESSAGE_HH 17 | #define SICK_LMS_1XX_MESSAGE_HH 18 | 19 | /* Macros */ 20 | #define SICK_LMS_1XX_MSG_HEADER_LEN (1) ///< Sick LMS 1xx message header length in bytes 21 | #define SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN (30000) ///< Sick LMS 1xx maximum payload length 22 | #define SICK_LMS_1XX_MSG_TRAILER_LEN (1) ///< Sick LMS 1xx length of the message trailer 23 | 24 | /* Definition dependencies */ 25 | #include 26 | #include 27 | #include "SickMessage.hh" 28 | #include "SickException.hh" 29 | 30 | /* Associate the namespace */ 31 | namespace SickToolbox { 32 | 33 | /** 34 | * \class SickLMS1xxMessage 35 | * \brief A class to represent all messages sent to and from the Sick LMS 1xx unit. 36 | */ 37 | class SickLMS1xxMessage : public SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN > { 38 | 39 | public: 40 | 41 | /** A standard constructor */ 42 | SickLMS1xxMessage( ); 43 | 44 | /** Constructs a packet by using BuildMessage */ 45 | SickLMS1xxMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ); 46 | 47 | /** Constructs a packet using ParseMessage() */ 48 | SickLMS1xxMessage( const uint8_t * const message_buffer ); 49 | 50 | /** Construct a well-formed raw packet */ 51 | void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ); 52 | 53 | /** Populates fields from a (well-formed) raw packet */ 54 | void ParseMessage( const uint8_t * const message_buffer ) throw ( SickIOException ); 55 | 56 | /** Get the length of the service code associated with the message */ 57 | std::string GetCommandType( ) const { return _command_type; } 58 | 59 | /** Get the service sub-code associated with the message */ 60 | std::string GetCommand( ) const { return _command; } 61 | 62 | /** Reset the data associated with this message (for initialization purposes) */ 63 | void Clear( ); 64 | 65 | /** A debugging function that prints the contents of the frame. */ 66 | void Print( ) const; 67 | 68 | /** Destructor */ 69 | ~SickLMS1xxMessage( ); 70 | 71 | private: 72 | 73 | /** Command type associated w/ message */ 74 | std::string _command_type; 75 | 76 | /** Command associated w/ message */ 77 | std::string _command; 78 | 79 | }; 80 | 81 | } /* namespace SickToolbox */ 82 | 83 | #endif /* SICK_LMS_1XX_MESSAGE_HH */ 84 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS1xxUtility.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS1xxUtility.hh 3 | * \brief Defines simple utility functions for working with the 4 | * Sick LMS 1xx laser range finder units. 5 | * 6 | * Code by Jason C. Derenick and Christopher R. Mansley. 7 | * Contact jasonder(at)seas(dot)upenn(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /* Auto-generated header */ 18 | #include "SickConfig.hh" 19 | 20 | /* Implementation Dependencies */ 21 | #include 22 | 23 | /** 24 | * \def REVERSE_BYTE_ORDER_16 25 | * \brief Reverses the byte order of the given 16 bit unsigned integer 26 | */ 27 | #define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) ) 28 | 29 | /** 30 | * \def REVERSE_BYTE_ORDER_32 31 | * \brief Reverses the byte order of the given 32 bit unsigned integer 32 | */ 33 | #define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) ) 34 | 35 | /* Associate the namespace */ 36 | namespace SickToolbox { 37 | 38 | #ifndef WORDS_BIGENDIAN 39 | 40 | /* NOTE: The following functions are necessary since the Sick LD doesn't adopt the 41 | * convention of converting from network byte order. 42 | */ 43 | 44 | /** 45 | * \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian) 46 | * \param value The 2-byte value to convert to little-endian 47 | * \return Value in Sick LMS byte order (little-endian) 48 | */ 49 | inline uint16_t host_to_sick_lms_1xx_byte_order( uint16_t value ) { 50 | return value; 51 | } 52 | 53 | /** 54 | * \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian) 55 | * \param value The 4-byte value to convert to little-endian 56 | * \return Value in Sick LMS byte order (little-endian) 57 | */ 58 | inline uint32_t host_to_sick_lms_1xx_byte_order( uint32_t value ) { 59 | return value; 60 | } 61 | 62 | /** 63 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian) 64 | * \param value The 2-byte value to convert to little-endian 65 | * \return Value in host byte order (little-endian) 66 | */ 67 | inline uint16_t sick_lms_1xx_to_host_byte_order( uint16_t value ) { 68 | return value; 69 | } 70 | 71 | /** 72 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian) 73 | * \param value The 4-byte value to convert to little-endian 74 | * \return Value in host byte order (little-endian) 75 | */ 76 | inline uint32_t sick_lms_1xx_to_host_byte_order( uint32_t value ) { 77 | return value; 78 | } 79 | 80 | #else // The host has a big-endian architecture 81 | 82 | /** 83 | * \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian) 84 | * \param value The 2-byte value to convert to little-endian 85 | * \return Value in Sick LMS byte order (little-endian) 86 | */ 87 | inline uint16_t host_to_sick_lms_1xx_byte_order( uint16_t value ) { 88 | return REVERSE_BYTE_ORDER_16(value); 89 | } 90 | 91 | /** 92 | * \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian) 93 | * \param value The 4-byte value to convertto little-endian 94 | * \return Value in Sick LMS byte order (little-endian) 95 | */ 96 | inline uint32_t host_to_sick_lms_1xx_byte_order( uint32_t value ) { 97 | return REVERSE_BYTE_ORDER_32(value); 98 | } 99 | 100 | /** 101 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian) 102 | * \param value The 2-byte value to convert to big-endian 103 | * \return Value in host byte order (big-endian) 104 | */ 105 | inline uint16_t sick_lms_1xx_to_host_byte_order( uint16_t value ) { 106 | return REVERSE_BYTE_ORDER_16(value); 107 | } 108 | 109 | /** 110 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian) 111 | * \param value The 4-byte value to convert to big-endian 112 | * \return Value in host byte order (big-endian) 113 | */ 114 | inline uint32_t sick_lms_1xx_to_host_byte_order( uint32_t value ) { 115 | return REVERSE_BYTE_ORDER_32(value); 116 | } 117 | 118 | #endif /* _LITTLE_ENDIAN_HOST */ 119 | 120 | /* 121 | * NOTE: Other utility functions can be defined here 122 | */ 123 | 124 | /** 125 | * \brief Utility function for converting int to standard string 126 | * \param value Integer to be converted 127 | * \return String representing the given integer 128 | */ 129 | inline std::string int_to_str( const int value ) { 130 | std::stringstream ss; 131 | ss << value; 132 | return ss.str(); 133 | } 134 | 135 | 136 | 137 | } //namespace SickToolbox 138 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS2xxBufferMonitor.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS2xxBufferMonitor.hh 3 | * \brief Defines a class for monitoring the receive 4 | * buffer when interfacing w/ a Sick LMS 2xx 5 | * laser range finder. 6 | * 7 | * Code by Jason C. Derenick and Thomas H. Miller. 8 | * Contact derenick(at)lehigh(dot)edu 9 | * 10 | * The Sick LIDAR Matlab/C++ Toolbox 11 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 12 | * All rights reserved. 13 | * 14 | * This software is released under a BSD Open-Source License. 15 | * See http://sicktoolbox.sourceforge.net 16 | */ 17 | 18 | #ifndef SICK_LMS_2XX_BUFFER_MONITOR_HH 19 | #define SICK_LMS_2XX_BUFFER_MONITOR_HH 20 | 21 | #define DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT (35000) ///< Max allowable time between consecutive bytes 22 | 23 | /* Definition dependencies */ 24 | #include "SickLMS2xxMessage.hh" 25 | #include "SickBufferMonitor.hh" 26 | #include "SickException.hh" 27 | 28 | /* Associate the namespace */ 29 | namespace SickToolbox { 30 | 31 | /*! 32 | * \brief A class for monitoring the receive buffer when interfacing with a Sick LMS LIDAR 33 | */ 34 | class SickLMS2xxBufferMonitor : public SickBufferMonitor< SickLMS2xxBufferMonitor, SickLMS2xxMessage > { 35 | 36 | public: 37 | 38 | /** A standard constructor */ 39 | SickLMS2xxBufferMonitor( ); 40 | 41 | /** A method for extracting a single message from the stream */ 42 | void GetNextMessageFromDataStream( SickLMS2xxMessage &sick_message ) throw( SickIOException ); 43 | 44 | /** A standard destructor */ 45 | ~SickLMS2xxBufferMonitor( ); 46 | 47 | }; 48 | 49 | } /* namespace SickToolbox */ 50 | 51 | #endif /* SICK_LMS_2XX_BUFFER_MONITOR_HH */ 52 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS2xxMessage.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS2xxMessage.hh 3 | * \brief Definition of class SickLMS2xxMessage. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #ifndef SICK_LMS_2XX_MESSAGE_HH 17 | #define SICK_LMS_2XX_MESSAGE_HH 18 | 19 | /* Definition dependencies */ 20 | #include 21 | #include 22 | #include "SickMessage.hh" 23 | #include "SickException.hh" 24 | 25 | #define CRC16_GEN_POL 0x8005 ///< Used to compute CRCs 26 | 27 | /** Makes a "short" in little endian */ 28 | #define MKSHORT(a,b) ((unsigned short) (a) | ((unsigned short)(b) << 8)) 29 | 30 | #define SICK_LMS_2XX_MSG_HEADER_LEN (4) ///< Sick LMS message length in bytes 31 | #define SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN (812) ///< Sick LMS max payload length in bytes 32 | #define SICK_LMS_2XX_MSG_TRAILER_LEN (2) ///< Sick LMS message trailer length in bytes 33 | 34 | /* Associate the namespace */ 35 | namespace SickToolbox { 36 | 37 | /** 38 | * \brief A class to represent all messages sent to and from the Sick LMS 2xx 39 | * 40 | * This class helps to construct messages to be sent to the Sick. It also 41 | * provides a container for received messages to be parsed into. 42 | */ 43 | class SickLMS2xxMessage : public SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN > 44 | { 45 | 46 | public: 47 | 48 | /** Default constructor. Constructs an empty message (not well-formed!). */ 49 | SickLMS2xxMessage( ); 50 | 51 | /** Constructs a frame by using BuildMessage(). */ 52 | SickLMS2xxMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length ); 53 | 54 | /** Constructs a frame using ParseMessage(). */ 55 | SickLMS2xxMessage( uint8_t * const message_buffer ); 56 | 57 | /** Constructs a well-formed raw frame from input fields. */ 58 | void BuildMessage( uint8_t dest_address, const uint8_t * const payload_buffer, 59 | const unsigned int payload_length ); 60 | 61 | /** Populates fields from a (well-formed) raw frame. */ 62 | void ParseMessage( const uint8_t * const message_buffer ); 63 | 64 | /** Gets the address of the frame. */ 65 | uint8_t GetDestAddress( ) const { return _message_buffer[1]; } 66 | 67 | /** Gets the command code associated with the message */ 68 | uint8_t GetCommandCode( ) const { return _message_buffer[MESSAGE_HEADER_LENGTH]; } 69 | 70 | /** Gets the status byte from an LMS response message (NOTE: only applies to Sick LMS response telegrams!) */ 71 | uint8_t GetStatusByte( ) const { return _message_buffer[MESSAGE_HEADER_LENGTH+_payload_length-1]; } 72 | 73 | /** Gets the checksum for the message. */ 74 | uint16_t GetChecksum( ) const { return _checksum; } 75 | 76 | /** Reset the data associated with this message (for initialization purposes) */ 77 | void Clear( ); 78 | 79 | /** A debugging function that prints the contents of the message. */ 80 | void Print( ) const; 81 | 82 | /** Destructor */ 83 | ~SickLMS2xxMessage(); 84 | 85 | protected: 86 | 87 | /** The checksum (CRC16) */ 88 | uint16_t _checksum; 89 | 90 | private: 91 | 92 | /** Computes the checksum of the frame. */ 93 | uint16_t _computeCRC( uint8_t * data, unsigned int data_length ) const; 94 | 95 | }; 96 | 97 | } /* namespace SickToolbox */ 98 | 99 | #endif //SICK_LMS_2XX_MESSAGE_HH 100 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickLMS2xxUtility.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickLMS2xxUtility.hh 3 | * \brief Defines simple utility functions for working with the 4 | * Sick LMS 2xx laser range finder units. 5 | * 6 | * Code by Jason C. Derenick and Thomas H. Miller. 7 | * Contact derenick(at)lehigh(dot)edu 8 | * 9 | * The Sick LIDAR Matlab/C++ Toolbox 10 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 11 | * All rights reserved. 12 | * 13 | * This software is released under a BSD Open-Source License. 14 | * See http://sicktoolbox.sourceforge.net 15 | */ 16 | 17 | /* Auto-generated header */ 18 | #include "SickConfig.hh" 19 | 20 | /** 21 | * \def REVERSE_BYTE_ORDER_16 22 | * \brief Reverses the byte order of the given 16 bit unsigned integer 23 | */ 24 | #define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) ) 25 | 26 | /** 27 | * \def REVERSE_BYTE_ORDER_32 28 | * \brief Reverses the byte order of the given 32 bit unsigned integer 29 | */ 30 | #define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) ) 31 | 32 | /* Associate the namespace */ 33 | namespace SickToolbox { 34 | 35 | #ifndef WORDS_BIGENDIAN 36 | 37 | /* NOTE: The following functions are necessary since the Sick LD doesn't adopt the 38 | * convention of converting from network byte order. 39 | */ 40 | 41 | /** 42 | * \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian) 43 | * \param value The 2-byte value to convert to little-endian 44 | * \return Value in Sick LMS byte order (little-endian) 45 | */ 46 | inline uint16_t host_to_sick_lms_2xx_byte_order( uint16_t value ) { 47 | return value; 48 | } 49 | 50 | /** 51 | * \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian) 52 | * \param value The 4-byte value to convert to little-endian 53 | * \return Value in Sick LMS byte order (little-endian) 54 | */ 55 | inline uint32_t host_to_sick_lms_2xx_byte_order( uint32_t value ) { 56 | return value; 57 | } 58 | 59 | /** 60 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian) 61 | * \param value The 2-byte value to convert to little-endian 62 | * \return Value in host byte order (little-endian) 63 | */ 64 | inline uint16_t sick_lms_2xx_to_host_byte_order( uint16_t value ) { 65 | return value; 66 | } 67 | 68 | /** 69 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian) 70 | * \param value The 4-byte value to convert to little-endian 71 | * \return Value in host byte order (little-endian) 72 | */ 73 | inline uint32_t sick_lms_2xx_to_host_byte_order( uint32_t value ) { 74 | return value; 75 | } 76 | 77 | #else // The host has a big-endian architecture 78 | 79 | /** 80 | * \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian) 81 | * \param value The 2-byte value to convert to little-endian 82 | * \return Value in Sick LMS byte order (little-endian) 83 | */ 84 | inline uint16_t host_to_sick_lms_2xx_byte_order( uint16_t value ) { 85 | return REVERSE_BYTE_ORDER_16(value); 86 | } 87 | 88 | /** 89 | * \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian) 90 | * \param value The 4-byte value to convertto little-endian 91 | * \return Value in Sick LMS byte order (little-endian) 92 | */ 93 | inline uint32_t host_to_sick_lms_2xx_byte_order( uint32_t value ) { 94 | return REVERSE_BYTE_ORDER_32(value); 95 | } 96 | 97 | /** 98 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian) 99 | * \param value The 2-byte value to convert to big-endian 100 | * \return Value in host byte order (big-endian) 101 | */ 102 | inline uint16_t sick_lms_2xx_to_host_byte_order( uint16_t value ) { 103 | return REVERSE_BYTE_ORDER_16(value); 104 | } 105 | 106 | /** 107 | * \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian) 108 | * \param value The 4-byte value to convert to big-endian 109 | * \return Value in host byte order (big-endian) 110 | */ 111 | inline uint32_t sick_lms_2xx_to_host_byte_order( uint32_t value ) { 112 | return REVERSE_BYTE_ORDER_32(value); 113 | } 114 | 115 | #endif /* _LITTLE_ENDIAN_HOST */ 116 | 117 | /* 118 | * NOTE: Other utility functions can be defined here 119 | */ 120 | 121 | } //namespace SickToolbox 122 | -------------------------------------------------------------------------------- /include/sicktoolbox/SickMessage.hh: -------------------------------------------------------------------------------- 1 | /*! 2 | * \file SickMessage.hh 3 | * \brief Defines the abstract parent class for all Sick messages. 4 | * 5 | * Code by Jason C. Derenick and Thomas H. Miller. 6 | * Contact derenick(at)lehigh(dot)edu 7 | * 8 | * The Sick LIDAR Matlab/C++ Toolbox 9 | * Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller 10 | * All rights reserved. 11 | * 12 | * This software is released under a BSD Open-Source License. 13 | * See http://sicktoolbox.sourceforge.net 14 | */ 15 | 16 | #ifndef SICK_MESSAGE 17 | #define SICK_MESSAGE 18 | 19 | /* Dependencies */ 20 | #include 21 | #include 22 | #include 23 | 24 | /* Associate the namespace */ 25 | namespace SickToolbox { 26 | 27 | /** 28 | * \class SickMessage 29 | * \brief Provides an abstract parent for all Sick messages 30 | */ 31 | template < unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 32 | class SickMessage { 33 | 34 | public: 35 | 36 | /** Some constants to make things more manageable */ 37 | static const unsigned int MESSAGE_HEADER_LENGTH = MSG_HEADER_LENGTH; 38 | static const unsigned int MESSAGE_TRAILER_LENGTH = MSG_TRAILER_LENGTH; 39 | static const unsigned int MESSAGE_PAYLOAD_MAX_LENGTH = MSG_PAYLOAD_MAX_LENGTH; 40 | static const unsigned int MESSAGE_MAX_LENGTH = MESSAGE_HEADER_LENGTH + MESSAGE_PAYLOAD_MAX_LENGTH + MESSAGE_TRAILER_LENGTH; 41 | 42 | /** A standard constructor */ 43 | SickMessage( ); 44 | 45 | /** Construct a well-formed Sick message */ 46 | void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ); 47 | 48 | /** Populates fields given a sequence of bytes representing a raw message */ 49 | virtual void ParseMessage( const uint8_t * const message_buffer ) = 0; 50 | 51 | /** Returns a copy of the raw message buffer */ 52 | void GetMessage( uint8_t * const message_buffer ) const; 53 | 54 | /** Resturns the total message length in bytes */ 55 | unsigned int GetMessageLength( ) const { return _message_length; } 56 | 57 | /** Returns a copy of the raw message payload */ 58 | void GetPayload( uint8_t * const payload_buffer ) const; 59 | 60 | /** Returns a copy of the payload as a C String */ 61 | void GetPayloadAsCStr( char * const payload_str ) const; 62 | 63 | /** Returns a subregion of the payload specified by indices */ 64 | void GetPayloadSubregion( uint8_t * const payload_sub_buffer, const unsigned int start_idx, 65 | const unsigned int stop_idx ) const; 66 | 67 | /** Returns the total payload length in bytes */ 68 | unsigned int GetPayloadLength( ) const { return _payload_length; } 69 | 70 | /** Indicates whether the message container is populated */ 71 | bool IsPopulated( ) const { return _populated; }; 72 | 73 | /** Clear the contents of the message container/object */ 74 | virtual void Clear( ); 75 | 76 | /** Print the contents of the message */ 77 | virtual void Print( ) const; 78 | 79 | /** A virtual destructor */ 80 | virtual ~SickMessage( ); 81 | 82 | protected: 83 | 84 | /** The length of the message payload in bytes */ 85 | unsigned int _payload_length; 86 | 87 | /** The length of the message in bytes */ 88 | unsigned int _message_length; 89 | 90 | /** The message as a raw sequence of bytes */ 91 | uint8_t _message_buffer[MESSAGE_MAX_LENGTH]; 92 | 93 | /** Indicates whether the message container/object is populated */ 94 | bool _populated; 95 | 96 | }; 97 | 98 | 99 | /** 100 | * \brief A default constructor 101 | */ 102 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 103 | SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::SickMessage( ) { } 104 | 105 | /** 106 | * \brief Constructs a Sick message given the parameter values 107 | * \param *payload_buffer The payload of the message as an array of bytes 108 | * \param payload_length The length of the payload in bytes 109 | */ 110 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 111 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) { 112 | 113 | /* Clear the object */ 114 | Clear(); 115 | 116 | /* Assign the payload and message lengths */ 117 | _payload_length = payload_length; 118 | _message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length; 119 | 120 | /* Copy the payload into the message buffer */ 121 | memcpy(&_message_buffer[MESSAGE_HEADER_LENGTH],payload_buffer,_payload_length); 122 | 123 | /* Mark the object container as being populated */ 124 | _populated = true; 125 | 126 | } 127 | 128 | /** 129 | * \brief Parses a sequence of bytes into a Sick message 130 | * \param *message_buffer A well-formed message to be parsed into the class' fields 131 | */ 132 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 133 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::ParseMessage( const uint8_t * const message_buffer ) { 134 | 135 | /* Clear the message container/object */ 136 | Clear(); 137 | 138 | /* Mark the object as populated */ 139 | _populated = true; 140 | } 141 | 142 | /** 143 | * \brief Get the message as a sequence of well-formed bytes 144 | * \param *message_buffer Destination buffer for message contents 145 | */ 146 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 147 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetMessage( uint8_t * const message_buffer ) const { 148 | memcpy(message_buffer,_message_buffer,_message_length); 149 | } 150 | 151 | /** 152 | * \brief Get the payload contents as a sequence of well-formed bytes 153 | * \param *payload_buffer Destination buffer for message payload contents 154 | */ 155 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 156 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayload( uint8_t * const payload_buffer ) const { 157 | memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length); 158 | } 159 | 160 | /** Returns a copy of the payload as a C String */ 161 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 162 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadAsCStr( char * const payload_buffer ) const { 163 | memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length); 164 | payload_buffer[_payload_length] = '\0'; 165 | } 166 | 167 | /** 168 | * \brief Get a specified sub-region of the payload buffer 169 | * \param *payload_sub_buffer Destination buffer for message payload contents 170 | * \param *start_idx The 0-indexed starting location for copying 171 | * \param *stop_idx The 0-indexed stopping location for copying 172 | */ 173 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 174 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadSubregion( uint8_t * const payload_sub_buffer, 175 | const unsigned int start_idx, 176 | const unsigned int stop_idx ) const { 177 | /* Extract the subregion */ 178 | memcpy(payload_sub_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH+start_idx],stop_idx+1); 179 | } 180 | 181 | /** 182 | * \brief Reset all internal fields and buffers 183 | */ 184 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 185 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Clear( ) { 186 | 187 | /* Reset the parent integer variables */ 188 | _message_length = _payload_length = 0; 189 | 190 | /* Clear the message buffer */ 191 | memset(_message_buffer,0,MESSAGE_MAX_LENGTH); 192 | 193 | /* Set the flag indicating this message object/container is empty */ 194 | _populated = false; 195 | } 196 | 197 | /** 198 | * \brief Print data about this object 199 | */ 200 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 201 | void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Print( ) const { 202 | 203 | std::cout << "Payload length: " << GetPayloadLength() << std::endl; 204 | std::cout << "Message length: " << GetMessageLength() << std::endl; 205 | std::cout << std::flush; 206 | 207 | std::cout << "Message (hex):" << std::endl; 208 | std::cout.setf(std::ios::hex,std::ios::basefield); 209 | for (unsigned int i = 0; i < _message_length; i++) { 210 | std::cout << (int)_message_buffer[i] << " "; 211 | } 212 | std::cout << std::endl << std::flush; 213 | 214 | std::cout << "Message (ASCII):" << std::endl; 215 | std::cout.setf(std::ios::dec,std::ios::basefield); 216 | for (unsigned int i = 0; i < _message_length; i++) { 217 | std::cout << _message_buffer[i] << " "; 218 | } 219 | std::cout << std::endl << std::flush; 220 | } 221 | 222 | /** 223 | * \brief A destructor 224 | */ 225 | template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH > 226 | SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::~SickMessage() { } 227 | 228 | } /* namespace SickToolbox */ 229 | 230 | #endif /* SICK_MESSAGE */ 231 | -------------------------------------------------------------------------------- /manuals/sicktoolbox-RS-422.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/sicktoolbox/9ad2fbf962e7ac46991ced5ce81a6536895bed6c/manuals/sicktoolbox-RS-422.pdf -------------------------------------------------------------------------------- /manuals/sicktoolbox-quickstart.pdf: -------------------------------------------------------------------------------- https://raw.githubusercontent.com/ros-drivers/sicktoolbox/9ad2fbf962e7ac46991ced5ce81a6536895bed6c/manuals/sicktoolbox-quickstart.pdf -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | sicktoolbox 4 | 1.0.104 5 | SICK Toolbox drivers for SICK laser rangefinders 6 | 7 | This package contains the ROS fork of the SICK LIDAR Matlab/C++ Toolbox, available from http://sicktoolbox.sourceforge.net/. 8 | 9 | The SICK LIDAR Matlab/C++ Toolbox offers stable and easy-to-use C++ drivers for SICK LMS 2xx and SICK LD LIDARs. Also included are config utilities, examples, and tutorials. 10 | 11 | 12 | Chad Rockey 13 | 14 | BSD 15 | 16 | http://sicktoolbox.sourceforge.net/ 17 | https://github.com/ros-drivers/sicktoolbox/issues 18 | https://github.com/ros-drivers/sicktoolbox 19 | 20 | Jason Derenick 21 | Thomas Miller 22 | 23 | catkin 24 | 25 | 26 | 27 | 28 | --------------------------------------------------------------------------------