xtdrone
/
XTDrone
1
0
Fork 15
Code Issues Pull Requests Releases Wiki Activity

add sicktoolbox

This commit is contained in:
robin_shaun 2021-02-27 08:21:50 +08:00
parent 3dcec7b6d3
commit f8a94e6054
85 changed files with 22445 additions and 0 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

43
sitl_config/ugv/sicktoolbox/AUTHORS Normal file
View File

@ -0,0 +1,43 @@
-------------------------------------------------------------------
-------------------------------------------------------------------
The Sick LIDAR Matlab/C++ Toolbox
-------------------------------------------------------------------
The Sick LIDAR Matlab/C++ Toolbox was originally developed
and is currently maintained by Jason Derenick and Thomas Miller
at Lehigh University. It is branched from the source code used
by the Ben Franklin Racing Team, whose robot car, "Little Ben",
was only one of six to successfully finish the 2007 DARPA Urban
Grand Challenge.
***AUTHORS
-----------------------------------------
Jason Derenick, Ph.D. Candidate
Computer Science and Engineering
Lehigh University
19 Memorial Drive West
Bethlehem, PA 18015 USA
derenick(at)lehigh(dot)edu
http://vader.cse.lehigh.edu/~derenick
-----------------------------------------
-----------------------------------------
Thomas Miller
Computer Science and Engineering
Lehigh University
19 Memorial Drive West
Bethlehem, PA 18015 USA
thm204(at)lehigh(dot)edu
http://www.lehigh.edu/~thm204
-----------------------------------------
Jason and Thomas are advised by Professor John Spletzer
in the Department of Computer Science and Engineering.
***ACKNOWLEDGEMENTS
Please see the THANKS file :o)
-------------------------------------------------------------------

96
sitl_config/ugv/sicktoolbox/CHANGELOG.rst Normal file
View File

@ -0,0 +1,96 @@
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
Changelog for package sicktoolbox
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
1.0.104 (2019-05-04)
--------------------
1.0.103 (2013-08-21)
--------------------
* No more Willow Garage email.
* Update CMakeLists.txt
* Contributors: Chad Rockey
1.0.102 (2013-03-28)
--------------------
* Update README
* Catkinized and converted to CMake.
* Initial package.xml and CmakeLists.txt
* Applied power_delay.patch.
* Applying unistd.patch
* Applied stdlib_include.patch
* Removing Matlab support.
* Commiting configured files for cmake conversion.
* Forgot an installation step for the INSTALL readme.
* Updated install files, updated install directions.
* Some minor changes
* Cleaned up and began adjusting mex interface a bit for the lms1xx
* Actually, the previous post should have been renamaing 1ms to lms2xx. Now I am adding lms1xx
* Added directory for lms1xx in matlab examples directory
* Added include for GCC4.3
* 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.
* deleting sicklms2xxlite
* Changed name of sickld-1.0 to sickld
* Moving mex lms dir to lms2xx
* fixed some bugs and restructured the driver to provide more low-level control
* Added range and reflectivity streaming; however, still can't figure out how to successfully transition between measurement modes without uninitializing. Lots of cleanup
* Adjusted the lms1xx app and some minor cleanup
* Added some range measurement acquisition to driver
* Polished configuration functions
* Adjusted lms1xx example file
* committing cahnges to base and sick ld & lms2xx drivers
* Fixed some bugs and setup configuration capability for the lms 1xx driver
* Started adding _getSickStatus and began writing the message parsing code for the Sick LMS 1xx - currently the LMS 1xx will compile but is unstable
* Setup connect/disconnect routines and buffer monitor for SickLMS1xx driver. Also, included an example for testing the code right now.
* Started adding driver files for SickLMS1xx unit
* 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)
* Moved c++/examples/lms to c++/examples/lms2xx
* Supplanted SickLMS with SickLMS2xx and sick_lms with sick_lms_2xx where appropriate. Currently compiles, but needs a sanity test
* Created sicklms1xx driver directory and Makefile.am for each directory (currently, they are empty)
* Updating configure.ac to use the new AX macros for lms1xx and lms2xx source directories
* changes sicklms-1.0 to sicklms2xx and sicklmslite-1.0 to sicklms1xx
* Adding macros for setting lms2xx and lms1xx source directories
* Changed lms driver directory to lms2xx and created lms1xx drive directory in c++/drivers
* 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.
* Fixed quotation bug for newer versions of autoconf/aclocal.
See http://www.gnu.org/software/autoconf/manual/html_node/Quoting-and-Parameters.html
* Added stdlib.h to ld_config's main.cc and lms_config's main.cc
*
*
*
*
* Added ld_lite directory
* Addedd lms_lite directory
*
*
* Added ld_lite folder for ld_lite driver
* Added lms_lite directory for lms_lite driver
* lmsmex.cc now supports up to four separate Sick LMS units.
* 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.
* 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.
* Changed std::cerr to std::cout for printing 'cancel buffer monitor' string
* Adjusted m-file comments for sickld and sicklms
* Adjusted m-file comments
* Fixing permissions
* Still fixing file permissions
* Adjusting file permissions
* Took out std::cerr in bad checksum exception handler.
* Took out std::cerr in bad checksum exception handler.
* Monitor now clears buffer on bad checksum
* Monitor now clears msg container on bad checksum
* Fixed print-out in build_mex script
* Took out packed attribute for structs in SickLMS.hh.
* Removed try/catch in buffer monitor base
* Adjusted the examples to exit more cleanly.
* Fixed rm -r bug in build_mex and install_mex
* Adjusted configure.ac
* Fixing README
* testing
* Adjusted NEWS
* Modified INSTALL
* Fixing permissions on matlab/install_mex.
* Fixing permissions with mex bash script.
* Changed dates in mex installation bash scripts.
* Removed doc directory from c++/drivers/ld.
* Initial project import.
* Contributors: Chad Rockey, Chris Mansley, Jason Derenick, Michael Sands, Tom Miller, chadrockey

121
sitl_config/ugv/sicktoolbox/CMakeLists.txt Normal file
View File

@ -0,0 +1,121 @@
cmake_minimum_required(VERSION 2.8.3)
project(sicktoolbox)
find_package(catkin REQUIRED)
## System dependencies are found with CMake's conventions
find_package(Threads REQUIRED)
###################################
## catkin specific configuration ##
###################################
## The catkin_package macro generates cmake config files for your package
## Declare things to be passed to dependent projects
## LIBRARIES: libraries you create in this project that dependent projects also need
## CATKIN_DEPENDS: catkin_packages dependent projects also need
## DEPENDS: system dependencies of this project that dependent projects also need
catkin_package(
INCLUDE_DIRS include
LIBRARIES SickLD SickLMS1xx SickLMS2xx
CATKIN_DEPENDS
DEPENDS Threads
)
###########
## Build ##
###########
## Specify additional locations of header files
## Your package locations should be listed before other locations
include_directories(include ${catkin_INCLUDE_DIRS})
# Driver libraries
add_library(SickLD c++/drivers/ld/sickld/SickLD.cc c++/drivers/ld/sickld/SickLDBufferMonitor.cc c++/drivers/ld/sickld/SickLDMessage.cc)
target_link_libraries(SickLD ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
add_library(SickLMS1xx c++/drivers/lms1xx/sicklms1xx/SickLMS1xx.cc c++/drivers/lms1xx/sicklms1xx/SickLMS1xxBufferMonitor.cc c++/drivers/lms1xx/sicklms1xx/SickLMS1xxMessage.cc)
target_link_libraries(SickLMS1xx ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
add_library(SickLMS2xx c++/drivers/lms2xx/sicklms2xx/SickLMS2xx.cc c++/drivers/lms2xx/sicklms2xx/SickLMS2xxBufferMonitor.cc c++/drivers/lms2xx/sicklms2xx/SickLMS2xxMessage.cc)
target_link_libraries(SickLMS2xx ${catkin_LIBRARIES} ${CMAKE_THREAD_LIBS_INIT})
# Examples
add_library(LDConfigLib c++/examples/ld/ld_config/src/ConfigFile.cpp)
add_executable(ld_config c++/examples/ld/ld_config/src/main.cc)
target_link_libraries(ld_config SickLD LDConfigLib ${catkin_LIBRARIES})
add_executable(ld_more_config c++/examples/ld/ld_more_config/src/main.cc)
target_link_libraries(ld_more_config SickLD ${catkin_LIBRARIES})
add_executable(ld_multi_sector c++/examples/ld/ld_multi_sector/src/main.cc)
target_link_libraries(ld_multi_sector SickLD ${catkin_LIBRARIES})
add_executable(ld_single_sector c++/examples/ld/ld_single_sector/src/main.cc)
target_link_libraries(ld_single_sector SickLD ${catkin_LIBRARIES})
add_executable(lms1xx_simple_app c++/examples/lms1xx/lms1xx_simple_app/src/main.cc)
target_link_libraries(lms1xx_simple_app SickLMS1xx ${catkin_LIBRARIES})
add_executable(lms2xx_config c++/examples/lms2xx/lms2xx_config/src/main.cc)
target_link_libraries(lms2xx_config SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_mean_values c++/examples/lms2xx/lms2xx_mean_values/src/main.cc)
target_link_libraries(lms2xx_mean_values SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_partial_scan c++/examples/lms2xx/lms2xx_partial_scan/src/main.cc)
target_link_libraries(lms2xx_partial_scan SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_real_time_indices c++/examples/lms2xx/lms2xx_real_time_indices/src/main.cc)
target_link_libraries(lms2xx_real_time_indices SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_set_variant c++/examples/lms2xx/lms2xx_set_variant/src/main.cc)
target_link_libraries(lms2xx_set_variant SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_simple_app c++/examples/lms2xx/lms2xx_simple_app/src/main.cc)
target_link_libraries(lms2xx_simple_app SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_stream_range_and_reflect c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/main.cc)
target_link_libraries(lms2xx_stream_range_and_reflect SickLMS2xx ${catkin_LIBRARIES})
add_executable(lms2xx_subrange c++/examples/lms2xx/lms2xx_subrange/src/main.cc)
target_link_libraries(lms2xx_subrange SickLMS2xx ${catkin_LIBRARIES})
#############
## Install ##
#############
# all install targets should use catkin DESTINATION variables
# See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html
## Mark executable scripts (Python etc.) for installation
## in contrast to setup.py, you can choose the destination
# install(PROGRAMS
# scripts/my_python_script
# DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
# )
## Mark executables and/or libraries for installation
install(TARGETS SickLD SickLMS1xx SickLMS2xx LDConfigLib ld_config
ld_more_config ld_multi_sector ld_single_sector lms1xx_simple_app
lms2xx_config lms2xx_mean_values lms2xx_partial_scan lms2xx_real_time_indices
lms2xx_set_variant lms2xx_simple_app lms2xx_stream_range_and_reflect lms2xx_subrange
ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
)
## TODO Move headers that aren't needed externally back into the source
## Mark cpp header files for installation
install(DIRECTORY include/${PROJECT_NAME}/
DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
FILES_MATCHING PATTERN "*.hh"
PATTERN ".svn" EXCLUDE
)
## TODO There are readmes, etc, create install rules for these
## Mark other files for installation (e.g. launch and bag files, etc.)
# install(FILES
# # myfile1
# # myfile2
# DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
# )

39
sitl_config/ugv/sicktoolbox/COPYING Normal file
View File

@ -0,0 +1,39 @@
--------------------------------------------------------------------
--------------------------------------------------------------------
The Sick LIDAR Matlab/C++ Toolbox (BSD) License
--------------------------------------------------------------------
The Sick LIDAR Matlab/C++ Toolbox
Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
All rights reserved.
Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions
are met:
* Redistributions of source code must retain the above
copyright notice, this list of conditions and the following
disclaimer.
* Redistributions in binary form must reproduce the above
copyright notice, this list of conditions and the following
disclaimer in the documentation and/or other materials
provided with the distribution.
* Neither the name(s) of the copyright holders nor the names
of its contributors may be used to endorse or promote products
derived from this software without specific prior written
permission.
THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
"AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
POSSIBILITY OF SUCH DAMAGE.
--------------------------------------------------------------------

23
sitl_config/ugv/sicktoolbox/README Normal file
View File

@ -0,0 +1,23 @@
-------------------------------------------------------------------
-------------------------------------------------------------------
The Sick LIDAR C++ Toolbox
-------------------------------------------------------------------
This is the ROS fork of the Sick LIDAR C++ Toolbox. If you're
looking for Matlab, please see the original at:
http://sicktoolbox.sourceforge.net/
*** Quick Start
To get up and running quickly with the toolbox, be sure to read
the quick start guide in the manuals directory. Additionally, to
enable 500Kbps communication via a USB-COMi-M be sure to see the
RS-422 tutorial in said directory.
*** Other Files
Please see the following documents for additional info:
COPYING - Information regarding the software license
AUTHORS - Information about the authors
THANKS - Acknowledgements
-------------------------------------------------------------------

31
sitl_config/ugv/sicktoolbox/THANKS Normal file
View File

@ -0,0 +1,31 @@
-------------------------------------------------------------------
-------------------------------------------------------------------
The Sick LIDAR Matlab/C++ Toolbox
-------------------------------------------------------------------
The authors would like to express their sincere gratitude to the
following members of the Ben Franklin Racing Team for their key
insights and help in developing the toolbox:
Aleksandr Kushleyev
Tully Foote
Alex Stewart
Jon Bohren
Brian Satterfield
Professor Daniel Lee
Professor John Spetzer
Additionally, we'd like to thank Douglas Paul for his work on the
initial Sick LMS 2xx serial driver.
*** Additional Acknowledgments
This software makes use of the following open-source
packages for its package configuration:
doxample - by Oren Ben-kiki
http://www.ben-kiki.org/oren/doxample
acx_pthread - by Steven G. Johnson
http://autoconf-archive.cryp.to/acx_pthread.html
-------------------------------------------------------------------

1
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/.deps/SickLD.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/.deps/SickLDBufferMonitor.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/.deps/SickLDMessage.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

4760
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/SickLD.cc Normal file
View File

File diff suppressed because it is too large Load Diff

132
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/SickLDBufferMonitor.cc Normal file
View File

@ -0,0 +1,132 @@
/*!
* \file SickLDBufferMonitor.cc
* \brief Implements a class for monitoring the receive
* buffer when interfacing w/ a Sick LD LIDAR.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iostream>
#include <sys/ioctl.h>
#include <sicktoolbox/SickLDBufferMonitor.hh>
#include <sicktoolbox/SickLDMessage.hh>
#include <sicktoolbox/SickException.hh>
#include <sicktoolbox/SickLDUtility.hh>
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A standard constructor
*/
SickLDBufferMonitor::SickLDBufferMonitor( ) : SickBufferMonitor< SickLDBufferMonitor, SickLDMessage >(this) { }
/**
* \brief Acquires the next message from the SickLD byte stream
* \param &sick_message The returned message object
*/
void SickLDBufferMonitor::GetNextMessageFromDataStream( SickLDMessage &sick_message ) throw( SickIOException ) {
/* Flush the input buffer */
uint8_t byte_buffer;
/* A buffer to hold the current byte out of the stream */
const uint8_t sick_response_header[4] = {0x02,'U','S','P'};
uint8_t checksum = 0;
uint8_t message_buffer[SickLDMessage::MESSAGE_MAX_LENGTH] = {0};
uint32_t payload_length = 0;
try {
/* Search for the header in the byte stream */
for (unsigned int i = 0; i < sizeof(sick_response_header);) {
/* Acquire the next byte from the stream */
_readBytes(&byte_buffer,1,DEFAULT_SICK_BYTE_TIMEOUT);
/* Check if the current byte matches the expected header byte */
if (byte_buffer == sick_response_header[i]) {
i++;
}
else {
i = 0;
}
}
/* Populate message buffer w/ response header */
memcpy(message_buffer,sick_response_header,4);
/* Acquire the payload length! */
_readBytes(&message_buffer[4],4,DEFAULT_SICK_BYTE_TIMEOUT);
/* Extract the payload size and adjust the byte order */
memcpy(&payload_length,&message_buffer[4],4);
payload_length = sick_ld_to_host_byte_order(payload_length);
/* Read the packet payload */
_readBytes(&message_buffer[8],payload_length,DEFAULT_SICK_BYTE_TIMEOUT);
/* Read the checksum */
_readBytes(&checksum,1,DEFAULT_SICK_BYTE_TIMEOUT);
/* Build the return message object based upon the received payload
* and compute the associated checksum.
*
* NOTE: In constructing this message we ignore the header bytes
* buffered since the BuildMessage routine will insert the
* correct header automatically and compute the payload's
* checksum for us. We could probably get away with using
* just ParseMessage here and not computing the checksum as
* we are using TCP. However, its safer this way.
*/
sick_message.BuildMessage(&message_buffer[SickLDMessage::MESSAGE_HEADER_LENGTH],payload_length);
/* Verify the checksum is correct (this is probably unnecessary since we are using TCP/IP) */
if (sick_message.GetChecksum() != checksum) {
throw SickBadChecksumException("SickLD::GetNextMessageFromDataStream: BAD CHECKSUM!!!");
}
/* Success */
}
catch(SickTimeoutException &sick_timeout) { /* This is ok! */ }
/* Catch a bad checksum! */
catch(SickBadChecksumException &sick_checksum_exception) {
sick_message.Clear(); // Clear the message container
}
/* Catch any serious IO buffer exceptions */
catch(SickIOException &sick_io_exception) {
throw;
}
/* A sanity check */
catch (...) {
throw;
}
}
/**
* \brief A standard destructor
*/
SickLDBufferMonitor::~SickLDBufferMonitor( ) { }
} /* namespace SickToolbox */

153
sitl_config/ugv/sicktoolbox/c++/drivers/ld/sickld/SickLDMessage.cc Normal file
View File

@ -0,0 +1,153 @@
/*!
* \file SickLDMessage.cc
* \brief Implements the class SickLDMessage.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iomanip>
#include <iostream>
#include <arpa/inet.h>
#include <sicktoolbox/SickLDMessage.hh>
#include <sicktoolbox/SickLDUtility.hh> // for byye-order conversions where necessary
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A default constructor
*/
SickLDMessage::SickLDMessage( ) :
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() {
/* Initialize the object */
Clear();
}
/**
* \brief Another constructor.
* \param *payload_buffer The payload for the packet as an array of bytes (including the header)
* \param payload_length The length of the payload array in bytes
*/
SickLDMessage::SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) :
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() {
/* Build the message object (implicit initialization) */
BuildMessage(payload_buffer,payload_length);
}
/**
* \brief Another constructor.
* \param *message_buffer A well-formed message to be parsed into the class' fields
*/
SickLDMessage::SickLDMessage( const uint8_t * const message_buffer ) :
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >() {
/* Parse the message into the container (implicit initialization) */
ParseMessage(message_buffer);
}
/**
* \brief Constructs a Sick LD message given parameter values
* \param *payload_buffer An address of the first byte to be copied into the message's payload
* \param payload_length The number of bytes to be copied into the message buffer
*/
void SickLDMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
/* Call the parent method
* NOTE: The parent method resets the object and assigns _message_length, _payload_length,
* _populated and copies the payload into the message buffer at the correct position
*/
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
::BuildMessage(payload_buffer,payload_length);
/*
* Set the message header!
*/
_message_buffer[0] = 0x02; // STX
_message_buffer[1] = 'U'; // User
_message_buffer[2] = 'S'; // Service
_message_buffer[3] = 'P'; // Protocol
/* Include the payload length in the header */
uint32_t payload_length_32 = host_to_sick_ld_byte_order((uint32_t)_payload_length);
memcpy(&_message_buffer[4],&payload_length_32,4);
/*
* Set the message trailer (just a checksum)!
*/
_message_buffer[_message_length-1] = _computeXOR(&_message_buffer[8],(uint32_t)_payload_length);
}
/**
* \brief Parses a sequence of bytes into a SickLDMessage object
* \param *message_buffer A well-formed message to be parsed into the class' fields
*/
void SickLDMessage::ParseMessage( const uint8_t * const message_buffer ) {
/* Call the parent method
* NOTE: This method resets the object and assigns _populated as true
*/
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >
::ParseMessage(message_buffer);
/* Extract the payload length */
uint32_t payload_length_32 = 0;
memcpy(&payload_length_32,&message_buffer[4],4);
_payload_length = (unsigned int)sick_ld_to_host_byte_order(payload_length_32);
/* Compute the total message length */
_message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
/* Copy the given packet into the buffer */
memcpy(_message_buffer,message_buffer,_message_length);
}
/**
* \brief Print the message contents.
*/
void SickLDMessage::Print( ) const {
std::cout.setf(std::ios::hex,std::ios::basefield);
std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;
std::cout << "Service code: " << (unsigned int) GetServiceCode() << std::endl;
std::cout << "Service subcode: " << (unsigned int) GetServiceSubcode() << std::endl;
std::cout << std::flush;
/* Call parent's print function */
SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN >::Print();
}
/**
* \brief Compute the message checksum (single-byte XOR).
* \param data The address of the first data element in a sequence of bytes to be included in the sum
* \param length The number of byte in the data sequence
*/
uint8_t SickLDMessage::_computeXOR( const uint8_t * const data, const uint32_t length ) {
/* Compute the XOR by summing all of the bytes */
uint8_t checksum = 0;
for (uint32_t i = 0; i < length; i++) {
checksum ^= data[i]; // NOTE: this is equivalent to simply summing all of the bytes
}
/* done */
return checksum;
}
SickLDMessage::~SickLDMessage( ) { }
} /* namespace SickToolbox */

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xx.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xxBufferMonitor.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/.deps/SickLMS1xxMessage.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

2294
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/SickLMS1xx.cc Normal file
View File

File diff suppressed because it is too large Load Diff

127
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/SickLMS1xxBufferMonitor.cc Normal file
View File

@ -0,0 +1,127 @@
/*!
* \file SickLMS1xxBufferMonitor.cc
* \brief Implements a class for monitoring the receive
* buffer when interfacing w/ a Sick LMS 1xx LIDAR.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iostream>
#include <sys/ioctl.h>
#include <sicktoolbox/SickLMS1xxBufferMonitor.hh>
#include <sicktoolbox/SickLMS1xxMessage.hh>
#include <sicktoolbox/SickLMS1xxUtility.hh>
#include <sicktoolbox/SickException.hh>
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A standard constructor
*/
SickLMS1xxBufferMonitor::SickLMS1xxBufferMonitor( ) : SickBufferMonitor< SickLMS1xxBufferMonitor, SickLMS1xxMessage >(this) { }
/**
* \brief Acquires the next message from the SickLMS1xx byte stream
* \param &sick_message The returned message object
*/
void SickLMS1xxBufferMonitor::GetNextMessageFromDataStream( SickLMS1xxMessage &sick_message ) throw( SickIOException ) {
/* Flush the input buffer */
uint8_t byte_buffer = 0;
uint8_t payload_buffer[SickLMS1xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
try {
/* Flush the TCP receive buffer */
_flushTCPRecvBuffer();
/* Search for STX in the byte stream */
do {
/* Grab the next byte from the stream */
_readBytes(&byte_buffer,1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT);
}
while (byte_buffer != 0x02);
/* Ok, now acquire the payload! (until ETX) */
int payload_length = 0;
do {
payload_length++;
_readBytes(&payload_buffer[payload_length-1],1,DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT);
}
while (payload_buffer[payload_length-1] != 0x03);
payload_length--;
/* Build the return message object based upon the received payload
* NOTE: In constructing this message we ignore the header bytes
* buffered since the BuildMessage routine will insert the
* correct header automatically and verify the message size
*/
sick_message.BuildMessage(payload_buffer,payload_length);
/* Success */
}
catch(SickTimeoutException &sick_timeout) { /* This is ok! */ }
/* Catch any serious IO buffer exceptions */
catch(SickIOException &sick_io_exception) {
throw;
}
/* A sanity check */
catch (...) {
throw;
}
}
/**
* \brief Flushes TCP receive buffer contents
*/
void SickLMS1xxBufferMonitor::_flushTCPRecvBuffer( ) const throw (SickIOException) {
char null_byte;
int num_bytes_waiting = 0;
/* Acquire number of awaiting bytes */
if (ioctl(_sick_fd,FIONREAD,&num_bytes_waiting)) {
throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!");
}
/* Flush awaiting bytes */
for (int i = 0; i < num_bytes_waiting; i++) {
/* Capture a single byte from the stream! */
if (read(_sick_fd,&null_byte,1) != 1) {
throw SickIOException("SickLMS1xxBufferMonitor::_flushTCPRecvBuffer: ioctl() failed!");
}
}
}
/**
* \brief A standard destructor
*/
SickLMS1xxBufferMonitor::~SickLMS1xxBufferMonitor( ) { }
} /* namespace SickToolbox */

201
sitl_config/ugv/sicktoolbox/c++/drivers/lms1xx/sicklms1xx/SickLMS1xxMessage.cc Normal file
View File

@ -0,0 +1,201 @@
/*!
* \file SickLMS1xxMessage.cc
* \brief Implements the class SickLMS1xxMessage.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iomanip>
#include <iostream>
#include <arpa/inet.h>
#include <sicktoolbox/SickLMS1xxMessage.hh>
#include <sicktoolbox/SickLMS1xxUtility.hh> // for byye-order conversions where necessary
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A default constructor
*/
SickLMS1xxMessage::SickLMS1xxMessage( ) :
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
_command_type(""),
_command("")
{
/* Initialize the object */
Clear();
}
/**
* \brief Another constructor.
* \param *payload_buffer The payload for the packet as an array of bytes (including the header)
* \param payload_length The length of the payload array in bytes
*/
SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) :
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
_command_type("Unknown"),
_command("Unknown")
{
/* Build the message object (implicit initialization) */
BuildMessage(payload_buffer,payload_length);
}
/**
* \brief Another constructor.
* \param *message_buffer A well-formed message to be parsed into the class' fields
*/
SickLMS1xxMessage::SickLMS1xxMessage( const uint8_t * const message_buffer ) :
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >(),
_command_type("Unknown"),
_command("Unknown")
{
/* Parse the message into the container (implicit initialization) */
ParseMessage(message_buffer);
}
/**
* \brief Constructs a well-formed Sick LMS 1xx message
* \param *payload_buffer An address of the first byte to be copied into the message's payload
* \param payload_length The number of bytes to be copied into the message buffer
*/
void SickLMS1xxMessage::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
/* Call the parent method
* NOTE: The parent method resets the object and assigns _message_length, _payload_length,
* _populated and copies the payload into the message buffer at the correct position
*/
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >
::BuildMessage(payload_buffer,payload_length);
/*
* Set the message header!
*/
_message_buffer[0] = 0x02; // STX
/*
* Set the message trailer!
*/
_message_buffer[_message_length-1] = 0x03; // ETX
/* Grab the (3-byte) command type */
char command_type[4] = {0};
for (int i = 0; i < 3; i++) {
command_type[i] = _message_buffer[i+1];
}
command_type[4] = '\0';
_command_type = command_type;
/* Grab the command (max length is 14 bytes) */
char command[15] = {0};
int i = 0;
for (; (i < 14) && (_message_buffer[5+i] != 0x20); i++) {
command[i] = _message_buffer[5+i];
}
command[i] = '\0';
_command = command;
}
/**
* \brief Parses a sequence of bytes into a SickLMS1xxMessage object
* \param *message_buffer A well-formed message to be parsed into the class' fields
*/
void SickLMS1xxMessage::ParseMessage( const uint8_t * const message_buffer ) throw (SickIOException) {
/* Call the parent method
* NOTE: This method resets the object and assigns _populated as true
*/
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >
::ParseMessage(message_buffer);
/* Compute the message length */
int i = 1;
const char * token = NULL;
while (message_buffer[i-1] != 0x03) {
if (i == 1) {
/* Grab the command type */
if ((token = strtok((char *)&_message_buffer[1]," ")) == NULL) {
throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!");
}
_command_type = token;
/* Grab the Command Code */
if ((token = strtok(NULL," ")) == NULL) {
throw SickIOException("SickLMS1xxMessage::ParseMessage: strtok() failed!");
}
_command = token;
}
i++; // Update message length
/* A sanity check */
if (i > SickLMS1xxMessage::MESSAGE_MAX_LENGTH) {
throw SickIOException("SickLMS1xxMessage::ParseMessage: Message Exceeds Max Message Length!");
}
}
/* Compute the total message length */
_payload_length = _message_length - MESSAGE_HEADER_LENGTH - MESSAGE_TRAILER_LENGTH;
/* Copy the given packet into the buffer */
memcpy(_message_buffer,message_buffer,_message_length);
}
/**
* \brief Reset all internal fields and buffers associated with the object.
*/
void SickLMS1xxMessage::Clear( ) {
/* Call the parent method and clear out class' protected members */
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Clear();
/* Reset the class' additional fields */
_command_type = "Unknown";
_command = "Unknown";
}
/**
* \brief Print the message contents.
*/
void SickLMS1xxMessage::Print( ) const {
//std::cout.setf(std::ios::hex,std::ios::basefield);
std::cout << "Command Type: " << GetCommandType() << std::endl;
std::cout << "Command Code: " << GetCommand() << std::endl;
std::cout << std::flush;
/* Call parent's print function */
SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN >::Print();
}
SickLMS1xxMessage::~SickLMS1xxMessage( ) { }
} /* namespace SickToolbox */

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xx.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xxBufferMonitor.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/.deps/SickLMS2xxMessage.Plo Normal file
View File

@ -0,0 +1 @@
# dummy

5203
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/SickLMS2xx.cc Normal file
View File

File diff suppressed because it is too large Load Diff

133
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/SickLMS2xxBufferMonitor.cc Normal file
View File

@ -0,0 +1,133 @@
/*!
* \file SickLMS2xxBufferMonitor.cc
* \brief Implements a class for monitoring the receive
* buffer when interfacing w/ a Sick LMS 2xx LIDAR.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iostream>
#include <termios.h>
#include <sicktoolbox/SickLMS2xx.hh>
#include <sicktoolbox/SickLMS2xxBufferMonitor.hh>
#include <sicktoolbox/SickLMS2xxMessage.hh>
#include <sicktoolbox/SickLMS2xxUtility.hh>
#include <sicktoolbox/SickException.hh>
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A standard constructor
*/
SickLMS2xxBufferMonitor::SickLMS2xxBufferMonitor( ) : SickBufferMonitor<SickLMS2xxBufferMonitor,SickLMS2xxMessage>(this) { }
/**
* \brief Acquires the next message from the SickLMS2xx byte stream
* \param &sick_message The returned message object
*/
void SickLMS2xxBufferMonitor::GetNextMessageFromDataStream( SickLMS2xxMessage &sick_message ) throw( SickIOException ) {
uint8_t search_buffer[2] = {0};
uint8_t payload_length_buffer[2] = {0};
uint8_t payload_buffer[SickLMS2xxMessage::MESSAGE_PAYLOAD_MAX_LENGTH] = {0};
uint8_t checksum_buffer[2] = {0};
uint16_t payload_length, checksum;
try {
/* Drain the I/O buffers! */
if (tcdrain(_sick_fd) != 0) {
throw SickIOException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: tcdrain failed!");
}
/* Read until we get a valid message header */
unsigned int bytes_searched = 0;
while(search_buffer[0] != 0x02 || search_buffer[1] != DEFAULT_SICK_LMS_2XX_HOST_ADDRESS) {
/* Slide the search window */
search_buffer[0] = search_buffer[1];
/* Attempt to read in another byte */
_readBytes(&search_buffer[1],1,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
/* Header should be no more than max message length + header length bytes away */
if (bytes_searched > SickLMS2xxMessage::MESSAGE_MAX_LENGTH + SickLMS2xxMessage::MESSAGE_HEADER_LENGTH) {
throw SickTimeoutException("SickLMS2xxBufferMonitor::GetNextMessageFromDataStream: header timeout!");
}
/* Increment the number of bytes searched */
bytes_searched++;
}
/* Read until we receive the payload length or we timeout */
_readBytes(payload_length_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
/* Extract the payload length */
memcpy(&payload_length,payload_length_buffer,2);
payload_length = sick_lms_2xx_to_host_byte_order(payload_length);
/* Make sure the payload length is legitimate, otherwise disregard */
if (payload_length <= SickLMS2xxMessage::MESSAGE_MAX_LENGTH) {
/* Read until we receive the payload or we timeout */
_readBytes(payload_buffer,payload_length,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
/* Read until we receive the checksum or we timeout */
_readBytes(checksum_buffer,2,DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT);
/* Copy into uint16_t so it can be used */
memcpy(&checksum,checksum_buffer,2);
checksum = sick_lms_2xx_to_host_byte_order(checksum);
/* Build a frame and compute the crc */
sick_message.BuildMessage(DEFAULT_SICK_LMS_2XX_HOST_ADDRESS,payload_buffer,payload_length);
/* See if the checksums match */
if(sick_message.GetChecksum() != checksum) {
throw SickBadChecksumException("SickLMS2xx::GetNextMessageFromDataStream: CRC16 failed!");
}
}
}
catch(SickTimeoutException &sick_timeout_exception) { /* This is ok! */ }
/* Handle a bad checksum! */
catch(SickBadChecksumException &sick_checksum_exception) {
sick_message.Clear(); // Clear the message container
}
/* Handle any serious IO exceptions */
catch(SickIOException &sick_io_exception) {
throw;
}
/* A sanity check */
catch (...) {
throw;
}
}
/**
* \brief A standard destructor
*/
SickLMS2xxBufferMonitor::~SickLMS2xxBufferMonitor( ) { }
} /* namespace SickToolbox */

181
sitl_config/ugv/sicktoolbox/c++/drivers/lms2xx/sicklms2xx/SickLMS2xxMessage.cc Normal file
View File

@ -0,0 +1,181 @@
/*!
* \file SickLMS2xxMessage.cc
* \brief Implementation of class SickFrame.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include <sicktoolbox/SickConfig.hh>
/* Implementation dependencies */
#include <iomanip>
#include <iostream>
#include <sicktoolbox/SickLMS2xxMessage.hh>
#include <sicktoolbox/SickLMS2xxUtility.hh>
/* Associate the namespace */
namespace SickToolbox {
/*!
* \brief A default constructor
*/
SickLMS2xxMessage::SickLMS2xxMessage( ) {
/* Initialize the object */
Clear();
}
/*!
* \brief A constructor for building a message from the given parameter values
* \param dest_address The source address of the message
* \param payload_buffer The payload of the message as an array of bytes (including the command code)
* \param payload_length The length of the payload array in bytes
*/
SickLMS2xxMessage::SickLMS2xxMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length ) :
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >() {
/* Build the message */
BuildMessage(dest_address,payload_buffer,payload_length);
}
/*!
* \brief A constructor for parsing a byte sequence into a message object
* \param message_buffer A well-formed message to be parsed into the class' fields
*/
SickLMS2xxMessage::SickLMS2xxMessage( uint8_t * const message_buffer ) :
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >() {
/* Parse the byte sequence into a message object */
ParseMessage(message_buffer);
}
/*!
* \brief Consructs a message object from given parameter values
* \param dest_address The destination address of the frame
* \param *payload_buffer The payload for the frame as an array of bytes (including the command code)
* \param payload_length The length of the payload array in bytes
*/
void SickLMS2xxMessage::BuildMessage( const uint8_t dest_address, const uint8_t * const payload_buffer,
const unsigned int payload_length ) {
/* Call the parent method!
* NOTE: The parent method resets the object and assigns _message_length, _payload_length,
* _populated and copies the payload into the message buffer at the correct position
*/
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >
::BuildMessage(payload_buffer,payload_length);
/*
* Set the message header!
*/
_message_buffer[0] = 0x02; // Start of transmission (stx)
_message_buffer[1] = dest_address; // Sick LMS address
/* Include the payload length in the header */
uint16_t payload_length_16 = host_to_sick_lms_2xx_byte_order((uint16_t)_payload_length);
memcpy(&_message_buffer[2],&payload_length_16,2);
/*
* Set the message trailer!
*/
_checksum = host_to_sick_lms_2xx_byte_order(_computeCRC(_message_buffer,_payload_length+4));
memcpy(&_message_buffer[_payload_length+4],&_checksum,2);
}
/*!
* \brief Parses a sequence of bytes into a well-formed message
* \param *message_buffer The buffer containing the source message
*/
void SickLMS2xxMessage::ParseMessage( const uint8_t * const message_buffer ) {
/* Call the parent method!
* NOTE: This method resets the object and assigns _populated as true
*/
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >
::ParseMessage(message_buffer);
/* Extract the payload length */
uint16_t payload_length_16 = 0;
memcpy(&payload_length_16,&message_buffer[2],2);
_payload_length = (unsigned int)sick_lms_2xx_to_host_byte_order(payload_length_16);
/* Compute the total message length */
_message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
/* Copy the give message into the buffer */
memcpy(_message_buffer, message_buffer,_message_length);
/* Extract the checksum from the frame */
memcpy(&_checksum,&_message_buffer[_payload_length+MESSAGE_HEADER_LENGTH],2);
_checksum = sick_lms_2xx_to_host_byte_order(_checksum);
}
/*!
* \brief Reset all internal fields and buffers associated with the object.
*/
void SickLMS2xxMessage::Clear( ) {
/* Call the parent method and clear out class' protected members */
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Clear();
/* Reset the class' additional fields */
_checksum = 0;
}
/*!
* \brief Print the message contents.
*/
void SickLMS2xxMessage::Print( ) const {
std::cout.setf(std::ios::hex,std::ios::basefield);
std::cout << "Checksum: " << (unsigned int) GetChecksum() << std::endl;
std::cout << "Dest. Addr.: " << (unsigned int) GetDestAddress() << std::endl;
std::cout << "Command Code: " << (unsigned int) GetCommandCode() << std::endl;
std::cout << std::flush;
/* Call parent's print function */
SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >::Print();
}
/*!
* \brief Computes the CRC16 of the given data buffer
* \param data An array of bytes whose checksum to compute
* \param len The length of the data array
* \return CRC16 computed over given data buffer
*/
uint16_t SickLMS2xxMessage::_computeCRC( uint8_t * data, unsigned int data_length ) const {
uint16_t uCrc16;
uint8_t abData[2];
uCrc16 = abData[0] = 0;
while (data_length-- ) {
abData[1] = abData[0];
abData[0] = *data++;
if(uCrc16 & 0x8000) {
uCrc16 = (uCrc16 & 0x7fff) << 1;
uCrc16 ^= CRC16_GEN_POL;
}
else {
uCrc16 <<= 1;
}
uCrc16 ^= MKSHORT(abData[0],abData[1]);
}
return uCrc16;
}
SickLMS2xxMessage::~SickLMS2xxMessage( ) { }
} /* namespace SickToolbox */

1227
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/Doxyfile Normal file
View File

File diff suppressed because it is too large Load Diff

21
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/conf/sickld.conf Normal file
View File

@ -0,0 +1,21 @@
###############################################################
#
# The Sick LIDAR Matlab/C++ Toolbox
#
# File: sickld.conf
# Auth: Jason Derenick and Thomas Miller at Lehigh University
# Cont: derenick(at)lehigh(dot)edu
# Date: 20 July 2007
#
# Desc: Sample config file for ld_config utility.
#
###############################################################
# Define the Sick LD motor speed (Hz)
SICK_LD_MOTOR_SPEED = 10
# Define the Sick LD scan res. (angle step) in degrees
SICK_LD_SCAN_RESOLUTION = 0.5
# Define the active scan areas for the device
SICK_LD_SCAN_AREAS = [90 270] [315 45]

3
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/doxygen-html-header Normal file
View File

@ -0,0 +1,3 @@
<meta http-equiv="Content-Type" content="text/html;charset=UTF-8">
<link href="doxygen.css" rel="stylesheet" type="text/css">
<link href="tabs.css" rel="stylesheet" type="text/css">

1
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/src/.deps/ConfigFile.Po Normal file
View File

@ -0,0 +1 @@
# dummy

1
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

142
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/src/ConfigFile.cpp Normal file
View File

@ -0,0 +1,142 @@
// ConfigFile.cpp
#include "ConfigFile.h"
using std::string;
ConfigFile::ConfigFile( string filename, string delimiter,
string comment, string sentry )
: myDelimiter(delimiter), myComment(comment), mySentry(sentry)
{
// Construct a ConfigFile, getting keys and values from given file
std::ifstream in( filename.c_str() );
if( !in ) throw file_not_found( filename );
in >> (*this);
}
ConfigFile::ConfigFile()
: myDelimiter( string(1,'=') ), myComment( string(1,'#') )
{
// Construct a ConfigFile without a file; empty
}
void ConfigFile::remove( const string& key )
{
// Remove key and its value
myContents.erase( myContents.find( key ) );
return;
}
bool ConfigFile::keyExists( const string& key ) const
{
// Indicate whether key is found
mapci p = myContents.find( key );
return ( p != myContents.end() );
}
/* static */
void ConfigFile::trim( string& s )
{
// Remove leading and trailing whitespace
static const char whitespace[] = " \n\t\v\r\f";
s.erase( 0, s.find_first_not_of(whitespace) );
s.erase( s.find_last_not_of(whitespace) + 1U );
}
std::ostream& operator<<( std::ostream& os, const ConfigFile& cf )
{
// Save a ConfigFile to os
for( ConfigFile::mapci p = cf.myContents.begin();
p != cf.myContents.end();
++p )
{
os << p->first << " " << cf.myDelimiter << " ";
os << p->second << std::endl;
}
return os;
}
std::istream& operator>>( std::istream& is, ConfigFile& cf )
{
// Load a ConfigFile from is
// Read in keys and values, keeping internal whitespace
typedef string::size_type pos;
const string& delim = cf.myDelimiter; // separator
const string& comm = cf.myComment; // comment
const string& sentry = cf.mySentry; // end of file sentry
const pos skip = delim.length(); // length of separator
string nextline = ""; // might need to read ahead to see where value ends
while( is || nextline.length() > 0 )
{
// Read an entire line at a time
string line;
if( nextline.length() > 0 )
{
line = nextline; // we read ahead; use it now
nextline = "";
}
else
{
std::getline( is, line );
}
// Ignore comments
line = line.substr( 0, line.find(comm) );
// Check for end of file sentry
if( sentry != "" && line.find(sentry) != string::npos ) return is;
// Parse the line if it contains a delimiter
pos delimPos = line.find( delim );
if( delimPos < string::npos )
{
// Extract the key
string key = line.substr( 0, delimPos );
line.replace( 0, delimPos+skip, "" );
// See if value continues on the next line
// Stop at blank line, next line with a key, end of stream,
// or end of file sentry
bool terminate = false;
while( !terminate && is )
{
std::getline( is, nextline );
terminate = true;
string nlcopy = nextline;
ConfigFile::trim(nlcopy);
if( nlcopy == "" ) continue;
nextline = nextline.substr( 0, nextline.find(comm) );
if( nextline.find(delim) != string::npos )
continue;
if( sentry != "" && nextline.find(sentry) != string::npos )
continue;
nlcopy = nextline;
ConfigFile::trim(nlcopy);
if( nlcopy != "" ) line += "\n";
line += nextline;
terminate = false;
}
// Store key and value
ConfigFile::trim(key);
ConfigFile::trim(line);
cf.myContents[key] = line; // overwrites if key is repeated
}
}
return is;
}

253
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/src/ConfigFile.h Normal file
View File

@ -0,0 +1,253 @@
// ConfigFile.h
// Class for reading named values from configuration files
// Richard J. Wagner v2.1 24 May 2004 wagnerr@umich.edu
// Copyright (c) 2004 Richard J. Wagner
//
// Permission is hereby granted, free of charge, to any person obtaining a copy
// of this software and associated documentation files (the "Software"), to
// deal in the Software without restriction, including without limitation the
// rights to use, copy, modify, merge, publish, distribute, sublicense, and/or
// sell copies of the Software, and to permit persons to whom the Software is
// furnished to do so, subject to the following conditions:
//
// The above copyright notice and this permission notice shall be included in
// all copies or substantial portions of the Software.
//
// THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
// IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
// FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
// AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
// LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING
// FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS
// IN THE SOFTWARE.
// Typical usage
// -------------
//
// Given a configuration file "settings.inp":
// atoms = 25
// length = 8.0 # nanometers
// name = Reece Surcher
//
// Named values are read in various ways, with or without default values:
// ConfigFile config( "settings.inp" );
// int atoms = config.read<int>( "atoms" );
// double length = config.read( "length", 10.0 );
// string author, title;
// config.readInto( author, "name" );
// config.readInto( title, "title", string("Untitled") );
//
// See file example.cpp for more examples.
#ifndef CONFIGFILE_H
#define CONFIGFILE_H
#include <string>
#include <map>
#include <iostream>
#include <fstream>
#include <sstream>
using std::string;
class ConfigFile {
// Data
protected:
string myDelimiter; // separator between key and value
string myComment; // separator between value and comments
string mySentry; // optional string to signal end of file
std::map<string,string> myContents; // extracted keys and values
typedef std::map<string,string>::iterator mapi;
typedef std::map<string,string>::const_iterator mapci;
// Methods
public:
ConfigFile( string filename,
string delimiter = "=",
string comment = "#",
string sentry = "EndConfigFile" );
ConfigFile();
// Search for key and read value or optional default value
template<class T> T read( const string& key ) const; // call as read<T>
template<class T> T read( const string& key, const T& value ) const;
template<class T> bool readInto( T& var, const string& key ) const;
template<class T>
bool readInto( T& var, const string& key, const T& value ) const;
// Modify keys and values
template<class T> void add( string key, const T& value );
void remove( const string& key );
// Check whether key exists in configuration
bool keyExists( const string& key ) const;
// Check or change configuration syntax
string getDelimiter() const { return myDelimiter; }
string getComment() const { return myComment; }
string getSentry() const { return mySentry; }
string setDelimiter( const string& s )
{ string old = myDelimiter; myDelimiter = s; return old; }
string setComment( const string& s )
{ string old = myComment; myComment = s; return old; }
// Write or read configuration
friend std::ostream& operator<<( std::ostream& os, const ConfigFile& cf );
friend std::istream& operator>>( std::istream& is, ConfigFile& cf );
protected:
template<class T> static string T_as_string( const T& t );
template<class T> static T string_as_T( const string& s );
static void trim( string& s );
// Exception types
public:
struct file_not_found {
string filename;
file_not_found( const string& filename_ = string() )
: filename(filename_) {} };
struct key_not_found { // thrown only by T read(key) variant of read()
string key;
key_not_found( const string& key_ = string() )
: key(key_) {} };
};
/* static */
template<class T>
string ConfigFile::T_as_string( const T& t )
{
// Convert from a T to a string
// Type T must support << operator
std::ostringstream ost;
ost << t;
return ost.str();
}
/* static */
template<class T>
T ConfigFile::string_as_T( const string& s )
{
// Convert from a string to a T
// Type T must support >> operator
T t;
std::istringstream ist(s);
ist >> t;
return t;
}
/* static */
template<>
inline string ConfigFile::string_as_T<string>( const string& s )
{
// Convert from a string to a string
// In other words, do nothing
return s;
}
/* static */
template<>
inline bool ConfigFile::string_as_T<bool>( const string& s )
{
// Convert from a string to a bool
// Interpret "false", "F", "no", "n", "0" as false
// Interpret "true", "T", "yes", "y", "1", "-1", or anything else as true
bool b = true;
string sup = s;
for( string::iterator p = sup.begin(); p != sup.end(); ++p )
*p = toupper(*p); // make string all caps
if( sup==string("FALSE") || sup==string("F") ||
sup==string("NO") || sup==string("N") ||
sup==string("0") || sup==string("NONE") )
b = false;
return b;
}
template<class T>
T ConfigFile::read( const string& key ) const
{
// Read the value corresponding to key
mapci p = myContents.find(key);
if( p == myContents.end() ) throw key_not_found(key);
return string_as_T<T>( p->second );
}
template<class T>
T ConfigFile::read( const string& key, const T& value ) const
{
// Return the value corresponding to key or given default value
// if key is not found
mapci p = myContents.find(key);
if( p == myContents.end() ) return value;
return string_as_T<T>( p->second );
}
template<class T>
bool ConfigFile::readInto( T& var, const string& key ) const
{
// Get the value corresponding to key and store in var
// Return true if key is found
// Otherwise leave var untouched
mapci p = myContents.find(key);
bool found = ( p != myContents.end() );
if( found ) var = string_as_T<T>( p->second );
return found;
}
template<class T>
bool ConfigFile::readInto( T& var, const string& key, const T& value ) const
{
// Get the value corresponding to key and store in var
// Return true if key is found
// Otherwise set var to given default
mapci p = myContents.find(key);
bool found = ( p != myContents.end() );
if( found )
var = string_as_T<T>( p->second );
else
var = value;
return found;
}
template<class T>
void ConfigFile::add( string key, const T& value )
{
// Add a key with given value
string v = T_as_string( value );
trim(key);
trim(v);
myContents[key] = v;
return;
}
#endif // CONFIGFILE_H
// Release notes:
// v1.0 21 May 1999
// + First release
// + Template read() access only through non-member readConfigFile()
// + ConfigurationFileBool is only built-in helper class
//
// v2.0 3 May 2002
// + Shortened name from ConfigurationFile to ConfigFile
// + Implemented template member functions
// + Changed default comment separator from % to #
// + Enabled reading of multiple-line values
//
// v2.1 24 May 2004
// + Made template specializations inline to avoid compiler-dependent linkage
// + Allowed comments within multiple-line values
// + Enabled blank line termination for multiple-line values
// + Added optional sentry to detect end of configuration file
// + Rewrote messy trimWhitespace() function as elegant trim()

430
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_config/src/main.cc Normal file
View File

@ -0,0 +1,430 @@
/*!
* \file main.cc
* \brief A simple configuration utility for Sick LD LIDARs.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* ConfigFile was written by Richard J. Wagner
* Download at http://www-personal.umich.edu/~wagnerr/ChE.html
*
* LDConfig is a simple utility to make configuring the SICK LD-LRS
* easier. By editing the configuration file, one can set up the LD-LRS
* in different configurations, without mucking about in source code.
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <string>
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <sicktoolbox/SickLD.hh>
#include <stdlib.h>
#include "ConfigFile.h"
#define INVALID_OPTION_STRING " Invalid option!!! :o("
#define PROMPT_STRING "ld?> "
/* Config file parameters */
#define CONFIG_OPT_MOTOR_SPD_STR "SICK_LD_MOTOR_SPEED"
#define CONFIG_OPT_SCAN_AREA_STR "SICK_LD_SCAN_AREAS"
#define CONFIG_OPT_SCAN_RES_STR "SICK_LD_SCAN_RESOLUTION"
using namespace std;
using namespace SickToolbox;
/**
* \fn sigintHandler
* \brief Callback for SIGINT interrupt. Used for uninitialization.
* \param signal Signal ID
*/
void sigintHandler(int signal);
/**
* \fn getUserOption
* \brief A utility function for capturing input from the user
* \param is_null_input Indicates whether the user entered null input
* \return User option as an integer
*/
int getUserOption(bool &is_null_input);
/**
* \fn getFilename
* \brief Prompts the user for config filename
* \return Configuration file path
*/
string getFilename();
/**
* \fn setConfig
* \brief Attempts to set the desired config via the driver.
* Parses configuration file as well.
*/
void setConfig();
/**
* \fn printConfig
* \brief Prints the current Sick status/config
*/
void printConfig();
/**
* \fn parseScanAreasStr
* \brief Parses the scan areas input string.
* \param start_angs Array of angles to begin a sector
* \param stop_angs Array of angles to end a sector
* \param areas String of sectors (e.g. "[0 10.75] [40.5 60.0] [85 100]")
* \param num_pairs Number of angle pairs found
* \return true on success, false otherwise
*
* pre: all parameters != NULL
* post: start_angs and stop_angs hold num_pairs values;
* 0 < num_pairs < SickLD::SICK_MAX_NUM_MEASURING_SECTORS
*
* This function separates the scan areas found in the config file
* into two arrays - one with angles that the scan areas begin at,
* and another with the angles that the scan areas end at. Uses
* setAngle()
*/
bool parseScanAreasStr(string& scan_areas_str, double *start_angs, double *stop_angs, int& num_sectors);
/**
* \fn parseNumStr
* \param entry String to be converted to a number
* \param num Variable to receive converted string
* \return true on success, false otherwise
*
* Parses entry string into a double, and saves it in num.
*/
bool parseNumStr(const string& entry, double& num);
/* A pointer to the current driver instance */
SickLD *sick_ld = NULL;
int main(int argc, char* argv[])
{
string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS); // IP address of the Sick LD unit
/* Check the num args */
if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cerr << "Usage: ld_config [SICK IP ADDRESS]" << endl
<< "Ex. ld_config 192.168.1.11" << endl;
return -1;
}
/* Assign the IP address */
if(argc == 2) {
sick_ip_addr = argv[1];
}
/* Instantiate the SickLD driver */
sick_ld = new SickLD(sick_ip_addr);
cout << endl;
cout << "The Sick LIDAR C++/Matlab Toolbox " << endl;
cout << "Sick LD Config Utility " << endl;
cout << endl;
/* Initialize the Sick LD */
try {
sick_ld->Initialize();
}
catch(...) {
cerr << "Initialize failed! Are you using the correct IP address?" << endl;
return -1;
}
/* Register the signal handler */
signal(SIGINT,sigintHandler);
do {
cout << "Enter your choice: (Ctrl-c to exit)" << endl;
cout << " [1] Set new configuration"<< endl;
cout << " [2] Show current settings"<< endl;
cout << PROMPT_STRING;
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
setConfig();
break;
case 2:
printConfig();
break;
default:
if(!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
cout << endl;
} while(true);
/* Success */
return 0;
}
/**
* Handles the SIGINT signal
*/
void sigintHandler(int signal) {
cout << endl;
cout << "Quitting..." << endl;
/* Unitialize the device */
try {
sick_ld->Uninitialize();
delete sick_ld;
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
exit(-1);
}
cout << endl;
cout << "Thanks for using the Sick LIDAR Matlab/C++ Toolbox!" << endl;
cout << "Bye Bye :o)" << endl;
exit(0);
}
/**
* Reads user input string and returns numeric representation.
*/
int getUserOption(bool &is_null_input) {
string user_input_str;
getline(cin,user_input_str);
// Check whether its null input
is_null_input = true;
if (user_input_str.length() > 0) {
is_null_input = false;
}
int int_val = 0;
istringstream input_stream(user_input_str);
input_stream >> int_val;
return int_val;
}
/**
* Prompts the user for the filename
*/
string getFilename() {
string filename;
cout << "config file: ";
getline(cin,filename);
return filename;
}
/**
* Parses config file and loads new configuration
*/
void setConfig() {
int motor_spd;
int num_sectors;
double scan_res;
double start_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0};
double stop_angs[SickLD::SICK_MAX_NUM_MEASURING_SECTORS] = {0};
string scan_areas_str;
ConfigFile sick_config_file; // Extracts values from config file
string config_path;
/* Prompt the user for the filename */
config_path = getFilename();
/* Instantiate the parser */
if(ifstream(config_path.c_str())) {
sick_config_file = ConfigFile(config_path);
}
else {
cerr << "Invalid filename!" << endl;
return;
}
/* Use the ConfigFile class to extract the various parameters for
* the sick configuration.
*
* NOTE: The third parameter specifies the value to use, if the second
* parameter is not found in the file.
*/
if(!sick_config_file.readInto(motor_spd,CONFIG_OPT_MOTOR_SPD_STR)) {
cerr << "ERROR: Invalid config file - " << CONFIG_OPT_MOTOR_SPD_STR << " needs to be specified!" << endl;
return;
}
if(!sick_config_file.readInto(scan_res,CONFIG_OPT_SCAN_RES_STR)) {
cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_RES_STR << " needs to be specified!" << endl;
return;
}
if(!sick_config_file.readInto(scan_areas_str,CONFIG_OPT_SCAN_AREA_STR)) {
cerr << "ERROR: Invalid config file - " << CONFIG_OPT_SCAN_AREA_STR << " needs to be specified!" << endl;
return;
}
/* Extract the start/stop pairs and angle res for the scanning sectors */
if (!parseScanAreasStr(scan_areas_str,start_angs,stop_angs,num_sectors)) {
cerr << "ERROR: Parser failed to extract scan areas!" << endl;
return;
}
/* Set the global parameters - we pass them all at once to ensure a feasible config */
cout << endl << "\tAttempting to configure the Sick LD..." << endl;
try {
sick_ld->SetSickGlobalParamsAndScanAreas(motor_spd,scan_res,start_angs,stop_angs,num_sectors);
}
catch(SickConfigException &sick_config_exception) {
cerr << "ERROR: Couldn't set requested configuration!" << endl;
return;
}
catch(...) {
exit(-1);
}
cout << "\t\tConfiguration Successfull!!!" << endl;
}
/**
* Print the current Sick LD configuration
*/
void printConfig() {
cout << endl;
sick_ld->PrintSickStatus();
sick_ld->PrintSickIdentity();
sick_ld->PrintSickGlobalConfig();
sick_ld->PrintSickEthernetConfig();
sick_ld->PrintSickSectorConfig();
}
/**
* Parses scan areas string in order extract desired sector configuration
*/
bool parseScanAreasStr(string& areas, double * start_angs, double * stop_angs, int& num_pairs) {
unsigned long i; // number of sectors found so far
unsigned int start_pos = 0; // starting position of a sector in 'areas'
unsigned int end_pos = 0; // ending position of a sector in 'areas'
unsigned int split = 0; // position of the delimiter inside a sector
string pair; // a string of 'areas' that contains one start and one stop angle
/* Get the beginning and end of the first scan sector */
start_pos = areas.find('[',start_pos); // Find the next [, starting at start_pos
end_pos = areas.find(']',start_pos);
/* Keep getting sectors until we either run out of open
* brackets or we exceed the number of allowed sectors
*/
for(i=0; (start_pos != (unsigned int)string::npos) && (i <= SickLD::SICK_MAX_NUM_MEASURING_SECTORS); i++) {
pair = areas.substr(start_pos+1,end_pos-(start_pos+1));
/* Eliminate any padding before first value */
try {
pair = pair.substr(pair.find_first_not_of(' '));
}
catch(...) {
cerr << "ERROR: There was an problem parsing your scan areas! Check your config file." << endl;
return false;
}
split = pair.find(' ');
/* Catch a lack of ' ' inside the sector definition;
* alternative is to let it fail at pair.substr(split)
*/
if(split == (unsigned int)string::npos) {
cerr << "ERROR: Invalid sector definition." << endl;
return false;
}
/* Get the number from the beginning to the delimiter */
if(!parseNumStr(pair.substr(0,split),start_angs[i])) {
cerr << "ERROR: Invalid start angle found." << endl;
return false;
}
/* Get the number from the delimiter to the end */
if(!parseNumStr(pair.substr(split),stop_angs[i])) {
cerr << "ERROR: Invalid stop angle found." << endl;
return false;
}
start_pos = end_pos; // Shift to the end of the current sector
/* Try to find another sector */
start_pos = areas.find("[", start_pos);
end_pos = areas.find("]", start_pos);
}
num_pairs = i;
/* Check if we broke out because of too many loops */
if(num_pairs > SickLD::SICK_MAX_NUM_MEASURING_SECTORS) {
cerr << "ERROR: Too many scan areas found (max " << SickLD::SICK_MAX_NUM_MEASURING_SECTORS << ")" << endl;
return false;
}
/* Or, we might not have even entered the loop */
if(num_pairs == 0) {
cerr << "ERROR: No scan areas found! Check brackets in your config file." << endl;
return false;
}
/* Success! */
return true;
}
/**
* Parses entry string into a double, and saves it in num.
*/
bool parseNumStr(const string& entry, double& num)
{
string num_str = entry.substr(entry.find_first_not_of(' '));
istringstream input_stream(num_str.c_str());
if(!(input_stream >> num)) {
cerr << "ERROR: Invalid angle value: " + num_str << endl;
return false;
}
/* Success! */
return true;
}

1
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_more_config/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

113
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_more_config/src/main.cc Normal file
View File

@ -0,0 +1,113 @@
/*!
* \file main.cc
* \brief Illustrates a variety of ways to configure the flash
* parameters on the Sick LD device as well as how to set
* the unit's clock.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLD.hh>
using namespace std;
using namespace SickToolbox;
int main (int argc, char *argv[]) {
/* A string for the IP address */
string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS);
/* Check the num of args */
if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cerr << "Usage: ld_more_config [SICK IP ADDRESS]" << endl
<< "Ex. ld_more_config 192.168.1.11" << endl;
return -1;
}
/* Assign the IP address */
if(argc == 2) {
sick_ip_addr = argv[1];
}
/* Define the object */
SickLD sick_ld(sick_ip_addr);
/*
* Initialize the Sick LD
*/
try {
sick_ld.Initialize();
}
catch(...) {
cerr << "Initialize failed! Are you using the correct IP address?" << endl;
return -1;
}
try {
/* Assign absolute and then relative time */
//uint16_t new_sick_time = 0;
//sick_ld.SetSickTimeAbsolute(1500,new_sick_time);
//cout << "\tNew sick time: " << new_sick_time << endl;
//sick_ld.SetSickTimeRelative(-500,new_sick_time);
//cout << "\tNew sick time: " << new_sick_time << endl;
/* Configure the Sick LD sensor ID */
//sick_ld.PrintSickGlobalConfig();
//sick_ld.SetSickSensorID(16);
//sick_ld.PrintSickGlobalConfig();
/* Configure the sick motor speed */
//sick_ld.PrintSickGlobalConfig();
//sick_ld.SetSickMotorSpeed(10);
//sick_ld.PrintSickGlobalConfig();
/* Configure the sick scan resolution */
//sick_ld.PrintSickGlobalConfig();
//sick_ld.SetSickScanResolution(0.5);
//sick_ld.PrintSickGlobalConfig();
/* Configure all the global parameters */
//double start_angle = 45;
//double stop_angle = 315;
//sick_ld.PrintSickGlobalConfig();
//sick_ld.PrintSickSectorConfig();
//sick_ld.SetSickGlobalParamsAndScanAreas(10,0.5,&start_angle,&stop_angle,1);
//sick_ld.PrintSickGlobalConfig();
//sick_ld.PrintSickSectorConfig();
}
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_ld.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

1
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_multi_sector/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

124
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_multi_sector/src/main.cc Normal file
View File

@ -0,0 +1,124 @@
/*!
* \file main.cc
* \brief Illustrates how to work with the Sick LD
* using multiple scan areas/sectors.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLD.hh>
/* Define the number of active sectors */
#define NUM_ACTIVE_SECTORS (3)
using namespace std;
using namespace SickToolbox;
int main (int argc, char *argv[]) {
/* A string for the IP address */
string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS);
/* Check the num of args */
if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cerr << "Usage: ld_multi_sector [SICK IP ADDRESS]" << endl
<< "Ex. ld_multi_sector 192.168.1.11" << endl;
return -1;
}
/* Assign the IP address */
if(argc == 2) {
sick_ip_addr = argv[1];
}
/* Define the temporal scan area (3 active sectors/scan areas)
* NOTE: This scan configuration will persist until the power
* is cycled or until it is reset w/ different params.
*/
double sector_start_angs[NUM_ACTIVE_SECTORS] = {45,270,345};
double sector_stop_angs[NUM_ACTIVE_SECTORS] = {90,315,15};
/* Define the destination buffers */
double range_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0};
unsigned int reflect_values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0};
/* Some buffer for additional info we can get from the Sick LD */
unsigned int num_values[NUM_ACTIVE_SECTORS] = {0};
unsigned int data_offsets[NUM_ACTIVE_SECTORS] = {0};
unsigned int sector_ids[NUM_ACTIVE_SECTORS] = {0};
/* Define the object */
SickLD sick_ld(sick_ip_addr);
/* Initialize the Sick LD */
try {
sick_ld.Initialize();
}
catch(...) {
cerr << "Initialize failed! Are you using the correct IP address?" << endl;
return -1;
}
try {
/* Set the temporary scan areas */
sick_ld.SetSickTempScanAreas(sector_start_angs,sector_stop_angs,NUM_ACTIVE_SECTORS);
/* Print the sector configuration */
sick_ld.PrintSickSectorConfig();
/* Request some measurements */
for (unsigned int i = 0; i < 10; i++) {
/* Acquire the most recent range and reflectivity (echo amplitude) values */
sick_ld.GetSickMeasurements(range_values,reflect_values,num_values,sector_ids,data_offsets);
/* Print out some data for each sector */
for (unsigned int i = 0; i < NUM_ACTIVE_SECTORS; i++) {
cout << "\t[Sector ID: " << sector_ids[i]
<< ", Num Meas: " << num_values[i]
<< ", 1st Range Val: " << range_values[data_offsets[i]]
<< ", 1st Reflect Val: " << reflect_values[data_offsets[i]]
<< "]" << endl;
}
cout << endl;
}
}
/* Catch any exception! */
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_ld.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success !*/
return 0;
}

1
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_single_sector/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

105
sitl_config/ugv/sicktoolbox/c++/examples/ld/ld_single_sector/src/main.cc Normal file
View File

@ -0,0 +1,105 @@
/*!
* \file main.cc
* \brief A simple application illustrating the use of
* the Sick LD C++ driver using a single sector.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLD.hh>
/* Use the namespace */
using namespace std;
using namespace SickToolbox;
int main (int argc, char *argv[]) {
/* A string for the IP address */
string sick_ip_addr(DEFAULT_SICK_IP_ADDRESS);
/* Check the num of args */
if(argc > 2 || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cerr << "Usage: ld_single_sector [SICK IP ADDRESS]" << endl
<< "Ex. ld_single_sector 192.168.1.11" << endl;
return -1;
}
/* Assign the IP address */
if(argc == 2) {
sick_ip_addr = argv[1];
}
/* Define the data buffers */
double values[SickLD::SICK_MAX_NUM_MEASUREMENTS] = {0};
unsigned int num_values = 0;
/* Define the bounds for a single sector */
double sector_start_ang = 90;
double sector_stop_ang = 270;
/* Define the object */
SickLD sick_ld(sick_ip_addr);
/*
* Initialize the Sick LD
*/
try {
sick_ld.Initialize();
}
catch(...) {
cerr << "Initialize failed! Are you using the correct IP address?" << endl;
return -1;
}
try {
/* Set the desired sector configuration */
sick_ld.SetSickTempScanAreas(&sector_start_ang,&sector_stop_ang,1);
/* Print the sector configuration */
sick_ld.PrintSickSectorConfig();
/* Acquire some range measurements */
for (unsigned int i = 0; i < 10; i++) {
/* Here we only want the range values so the second arg is NULL */
sick_ld.GetSickMeasurements(values,NULL,&num_values);
cout << "\t Num. Values: " << num_values << endl;
}
}
/* Catch any exceptions */
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_ld.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success !*/
return 0;
}

1
sitl_config/ugv/sicktoolbox/c++/examples/lms1xx/lms1xx_simple_app/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

94
sitl_config/ugv/sicktoolbox/c++/examples/lms1xx/lms1xx_simple_app/src/main.cc Normal file
View File

@ -0,0 +1,94 @@
/*!
* \file main.cc
* \brief A simple application using the Sick LMS 1xx driver.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS1xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[])
{
/*
* Instantiate an instance
*/
SickLMS1xx sick_lms_1xx;
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_1xx.Initialize();
}
catch(...) {
cerr << "Initialize failed! Are you using the correct IP address?" << endl;
return -1;
}
try {
unsigned int status = 1;
unsigned int num_measurements = 0;
unsigned int range_1_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS];
unsigned int range_2_vals[SickLMS1xx::SICK_LMS_1XX_MAX_NUM_MEASUREMENTS];
//sick_lms_1xx.SetSickScanFreqAndRes(SickLMS1xx::SICK_LMS_1XX_SCAN_FREQ_25,
//SickLMS1xx::SICK_LMS_1XX_SCAN_RES_25);
//sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_DIST_DOUBLE_PULSE,
// SickLMS1xx::SICK_LMS_1XX_REFLECT_NONE);
sick_lms_1xx.SetSickScanDataFormat(SickLMS1xx::SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT);
for (int i = 0; i < 1000; i++) {
sick_lms_1xx.GetSickMeasurements(range_1_vals,range_2_vals,range_1_vals,range_2_vals,num_measurements,&status);
std::cout << i << ": " << num_measurements << " " << status << std::endl;
}
}
catch(SickConfigException sick_exception) {
std::cout << sick_exception.what() << std::endl;
}
catch(SickIOException sick_exception) {
std::cout << sick_exception.what() << std::endl;
}
catch(SickTimeoutException sick_exception) {
std::cout << sick_exception.what() << std::endl;
}
catch(...) {
cerr << "An Error Occurred!" << endl;
return -1;
}
/*
* Uninitialize the device
*/
try {
sick_lms_1xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

14
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_config/README Normal file
View File

@ -0,0 +1,14 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_config
Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o)
Desc: This example provides a simple shell-like configuration
utility for working w/ Sick LMS 2xx LIDARs. It can be
used for setting measuring units, measuring mode, and
sensitivity. It can also display the current LMS config.
Example call (from build dir): ./lms_config /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_config/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

627
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_config/src/main.cc Normal file
View File

@ -0,0 +1,627 @@
/*!
* \file main.cc
* \brief A simple config utility for Sick LMS 2xx LIDARs.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Implementation dependencies */
#include <stdlib.h>
#include <string>
#include <sstream>
#include <iostream>
#include <signal.h>
#include <stdlib.h>
#include <sicktoolbox/SickLMS2xx.hh>
#define INVALID_OPTION_STRING " Invalid option!!! :o("
#define PROMPT_STRING "lms2xx?> "
using namespace std;
using namespace SickToolbox;
/**
* \fn sigintHandler
* \brief Callback for SIGINT interrupt. Used for uninitialization.
* \param signal Signal ID
*/
void sigintHandler(int signal);
/**
* \fn strToInt
* \brief Utility function for converting string to int
* \param input_str Input string to convert
*/
int strToInt(string input_str);
/**
* \fn getUserOption
* \brief Utility function for grabbing user input
* \param is_null_input Output parameter indicating whether
* user entered NULL input
*/
int getUserOption(bool &is_null_input);
/**
* \fn writeToEEPROM
* \brief Confirms the user actually wants to perform write
*/
bool writeToEEPROM();
/**
* \fn setMeasuringUnits
* \brief Prompts the user and sets the desired measuring
* units via the driver interface.
*/
void setMeasuringUnits();
/**
* \fn setMeasuringMode
* \brief Prompts the user and sets the desired measuring
* mode via the driver interface.
*/
void setMeasuringMode();
/**
* \fn setAvailabilityLevel
* \brief Prompts the user and sets the desired availability
* level via the driver interface.
*/
void setAvailabilityLevel();
/**
* \fn setSensitivityLevel
* \brief Prompts the user and sets the desired sensitivity
* level via the driver interface.
*/
void setSensitivityLevel();
/* A pointer to the Sick obj */
SickLMS2xx *sick_lms_2xx = NULL;
int main(int argc, char * argv[]) {
string device_str; // Device path of the Sick LMS 2xx
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_config PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_config /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/* Instantiate the SickLMS2xx class with the device path string. */
sick_lms_2xx = new SickLMS2xx(device_str);
cout << endl;
cout << "The Sick LIDAR C++/Matlab Toolbox " << endl;
cout << "Sick LMS 2xx Config Utility " << endl;
/* Initialize the device */
try {
sick_lms_2xx->Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/* Register the signal handler */
signal(SIGINT,sigintHandler);
cout << "NOTE - Configuring the Sick LMS - REQUIRES - writing to the device's EEPROM." << endl;
cout << " The number of times this can be done is finite (a few thousand)." << endl;
cout << " Thus, you should configure sparingly." << endl;
cout << endl;
do {
cout << "Enter your choice: (Ctrl-c to exit)" << endl;
cout << " [1] Set measuring units"<< endl;
cout << " [2] Set measuring mode"<< endl;
cout << " [3] Set availability level" << endl;
cout << " [4] Set sensitivity level" << endl;
cout << " [5] Show detailed configuration" << endl;
cout << PROMPT_STRING;
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
setMeasuringUnits();
break;
case 2:
setMeasuringMode();
break;
case 3:
setAvailabilityLevel();
break;
case 4:
setSensitivityLevel();
break;
case 5:
sick_lms_2xx->PrintSickConfig();
break;
default:
if (!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
cout << endl;
} while(true);
/* Success! */
return 0;
}
/**
* Handle the SIGINT signal
*/
void sigintHandler( int signal ) {
cout << endl;
cout << "Quitting..." << endl;
/* Unitialize the device */
try {
sick_lms_2xx->Uninitialize();
delete sick_lms_2xx;
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
exit(-1);
}
cout << endl;
cout << "Thanks for using the Sick LIDAR Matlab/C++ Toolbox!" << endl;
cout << "Bye Bye :o)" << endl;
exit(0);
}
/**
* Converts ascii string to an integer
*/
int strToInt( string input_str ) {
int int_val;
istringstream input_stream(input_str);
input_stream >> int_val;
return int_val;
}
/**
* Reads in user input string and returns numeric
* representation
*/
int getUserOption( bool &is_null_input ) {
string user_input_str;
getline(cin,user_input_str);
// Check whether its null input
is_null_input = true;
if (user_input_str.length() > 0) {
is_null_input = false;
}
return strToInt(user_input_str);
}
/**
* Prompts the user to confirm requested operation
*/
bool writeToEEPROM( ) {
string user_input_str;
do {
cout << "This will attempt to write to EEPROM. Continue [y/n]? ";
getline(cin,user_input_str);
// Check whether its null input
if (user_input_str == "Y" || user_input_str == "y") {
return true;
}
else if (user_input_str == "N" || user_input_str == "n") {
return false;
}
else {
cerr << "Please answer [y/n]!" << endl;
}
} while (true);
}
/**
* Sets the measuring units of the device
*/
void setMeasuringUnits() {
bool keep_running = true;
do {
cout << endl;
cout << "Select the desired units:" << endl;
cout << " [1] Centimeters (cm)"<< endl;
cout << " [2] Millimeters (mm)"<< endl;
cout << " [3] Back to main"<< endl;
cout << PROMPT_STRING;
try {
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
if (writeToEEPROM()) {
cout << " Setting Sick LMS units to (cm)" << endl;
sick_lms_2xx->SetSickMeasuringUnits(SickLMS2xx::SICK_MEASURING_UNITS_CM);
cout << " Done!" << endl;
}
break;
case 2:
if (writeToEEPROM()) {
cout << " Setting Sick LMS units to (mm)" << endl;
sick_lms_2xx->SetSickMeasuringUnits(SickLMS2xx::SICK_MEASURING_UNITS_MM);
cout << " Done!" << endl;
}
break;
case 3:
keep_running = !keep_running;
break;
default:
if (!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
}
catch( SickException &sick_exception ) {
cerr << "A Sick exception occurred!" << endl;
exit(-1);
}
catch(...) {
cerr << "An unknown exception occurred!" << endl;
exit(-1);
}
} while (keep_running);
}
/**
* Sets the measuring mode of the device
*/
void setMeasuringMode() {
bool keep_running = true;
do {
cout << endl;
cout << "Select the desired measuring mode (see telegram listing for descriptions):" << endl;
cout << " [1] Measurement range 8m/80m; field A, field B, and dazzle" << endl;
cout << " [2] Measurement range 8m/80m; reflector bits in 8 levels" << endl;
cout << " [3] Measurement range 8m/80m; field A, field B and field C" << endl;
cout << " [4] Measurement range 16m/theoretically 160m; reflector bits in 4 levels" << endl;
cout << " [5] Measurement range 16m/theoretically 160m; field A and field B" << endl;
cout << " [6] Measurement range 32m/theoretically 320m; reflector bits in 2 levels" << endl;
cout << " [7] Measurement range 32m/theoretically 320m; field A" << endl;
cout << " [8] Measurement range 32m/theoretically 320m; Immediate" << endl;
cout << " [9] Reflectivity/Intensity values" << endl;
cout << " [10] Back to main" << endl;
cout << PROMPT_STRING;
try {
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 8m/80m; field A, field B, and dazzle" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE);
cout << " Done!" << endl;
}
break;
case 2:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 8m/80m; reflector bits in 8 levels" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_8_OR_80_REFLECTOR);
cout << " Done!" << endl;
}
break;
case 3:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 8m/80m; field A, field B and field C" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_8_OR_80_FA_FB_FC);
cout << " Done!" << endl;
}
break;
case 4:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 16m/theoretically 160m; reflector bits in 4 levels" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_16_REFLECTOR);
cout << " Done!" << endl;
}
break;
case 5:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 16m/theoretically 160m; field A and field B" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_16_FA_FB);
cout << " Done!" << endl;
}
break;
case 6:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 32m/theoretically 320m; reflector bit in 2 levels" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_32_REFLECTOR);
cout << " Done!" << endl;
}
break;
case 7:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 32m/theoretically 320m; field A" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_32_FA);
cout << " Done!" << endl;
}
break;
case 8:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Measurement range 32m/theoretically 320m; Immediate" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_32_IMMEDIATE);
cout << " Done!" << endl;
}
break;
case 9:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Meas. Mode to: Reflectivity/Intensity" << endl;
sick_lms_2xx->SetSickMeasuringMode(SickLMS2xx::SICK_MS_MODE_REFLECTIVITY);
cout << " Done!" << endl;
}
break;
case 10:
keep_running = !keep_running;
break;
default:
if (!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
}
catch( SickException &sick_exception ) {
cerr << "A Sick exception occurred!" << endl;
exit(-1);
}
catch(...) {
cerr << "An unknown exception occurred!" << endl;
exit(-1);
}
} while (keep_running);
}
/**
* Sets the measuring mode of the device
*/
void setAvailabilityLevel() {
bool keep_running = true;
do {
cout << endl;
cout << "Select the desired availability (see telegram listing for descriptions):" << endl;
cout << " [1] Restore to factory default" << endl;
cout << " [2] High" << endl;
cout << " [3] High w/ Real-time indices" << endl;
cout << " [4] High w/ No effect dazzle" << endl;
cout << " [5] High w/ Real-time indices and No effect dazzle" << endl;
cout << " [6] Real-time indices" << endl;
cout << " [7] Real-time indices w/ No effect dazzle" << endl;
cout << " [8] No effect dazzle" << endl;
cout << " [9] Back to main" << endl;
cout << PROMPT_STRING;
try {
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: Factory settings" << endl;
sick_lms_2xx->SetSickAvailability();
cout << " Done!" << endl;
}
break;
case 2:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: High" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_HIGH);
cout << " Done!" << endl;
}
break;
case 3:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: High w/ Real-time indices" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_HIGH | SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES);
cout << " Done!" << endl;
}
break;
case 4:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: High w/ No effect dazzle" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_HIGH | SickLMS2xx::SICK_FLAG_AVAILABILITY_DAZZLE_NO_EFFECT);
cout << " Done!" << endl;
}
break;
case 5:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: High w/ Real-time indices and No effect dazzle" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_HIGH | SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES | SickLMS2xx::SICK_FLAG_AVAILABILITY_DAZZLE_NO_EFFECT);
cout << " Done!" << endl;
}
break;
case 6:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: Real-time indices" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES);
cout << " Done!" << endl;
}
break;
case 7:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: Real-time indices w/ No effect dazzle" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES | SickLMS2xx::SICK_FLAG_AVAILABILITY_DAZZLE_NO_EFFECT);
cout << " Done!" << endl;
}
break;
case 8:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Availability to: No effect dazzle" << endl;
sick_lms_2xx->SetSickAvailability(SickLMS2xx::SICK_FLAG_AVAILABILITY_DAZZLE_NO_EFFECT);
cout << " Done!" << endl;
}
break;
case 9:
keep_running = !keep_running;
break;
default:
if (!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
}
catch( SickException &sick_exception ) {
cerr << "A Sick exception occurred!" << endl;
exit(-1);
}
catch(...) {
cerr << "An unknown exception occurred!" << endl;
exit(-1);
}
} while (keep_running);
}
/**
* Sets the Sick LMS sensitivity level
*/
void setSensitivityLevel() {
bool keep_running = true;
do {
cout << endl;
cout << "Select the desired sensitivity level:" << endl;
cout << " [1] High (42m @ 10% reflectivity)"<< endl;
cout << " [2] Standard (30m @ 10% reflectivity, factory setting)"<< endl;
cout << " [3] Medium (25m @ 10% reflectivity)"<< endl;
cout << " [4] Low (20m @ 10% reflectivity)"<< endl;
cout << " [5] Back to main"<< endl;
cout << PROMPT_STRING;
try {
bool is_null_input;
switch(getUserOption(is_null_input)) {
case 1:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Sensitivity to: High" << endl;
sick_lms_2xx->SetSickSensitivity(SickLMS2xx::SICK_SENSITIVITY_HIGH);
cout << " Done!" << endl;
}
break;
case 2:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Sensitivity to: Standard (Factory setting)" << endl;
sick_lms_2xx->SetSickSensitivity();
cout << " Done!" << endl;
}
break;
case 3:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Sensitivity to: Medium" << endl;
sick_lms_2xx->SetSickSensitivity(SickLMS2xx::SICK_SENSITIVITY_MEDIUM);
cout << " Done!" << endl;
}
break;
case 4:
if (writeToEEPROM()) {
cout << " Setting Sick LMS Sensitivity to: Low" << endl;
sick_lms_2xx->SetSickSensitivity(SickLMS2xx::SICK_SENSITIVITY_LOW);
cout << " Done!" << endl;
}
break;
case 5:
keep_running = !keep_running;
break;
default:
if (!is_null_input) {
cerr << INVALID_OPTION_STRING << endl;
}
}
}
catch( SickException &sick_exception ) {
cerr << "A Sick exception occurred!" << endl;
exit(-1);
}
catch(...) {
cerr << "An unknown exception occurred!" << endl;
exit(-1);
}
} while (keep_running);
}

11
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_mean_values/README Normal file
View File

@ -0,0 +1,11 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_mean_values
Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o)
Desc: This example illustrates grabbing mean values from the Sick LMS.
Example call (from build dir): ./lms_mean_values /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_mean_values/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

114
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_mean_values/src/main.cc Normal file
View File

@ -0,0 +1,114 @@
/*!
* \file main.cc
* \brief A simple example illustrating how to acquire
* mean values from the Sick LMS 2xx.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[])
{
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh
unsigned int num_values; // Holds the number of measurements returned
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_mean_values PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_mean_values /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate an instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Acquire a few scans from the Sick LMS
*/
try {
for (unsigned int i=0; i < 10; i++) {
/*
* The first argument is an input to the driver
* telling it the number of raw scans over which
* the Sick should average. (i.e. here, the Sick
* will average over 5 scans before returning
* the resulting mean values)
*/
sick_lms_2xx.GetSickMeanValues(5,values,num_values);
cout << "\t Num. Values: " << num_values << endl;
}
}
/* Catch anything else and exit */
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

12
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_partial_scan/README Normal file
View File

@ -0,0 +1,12 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_partial_scan
Note: This example is NOT COMPATIBLE w/ LMS {211,221,291}-S14 LIDARs!
Desc: This example illustrates grabbing partial scans using the
Sick LMS driver.
Example call (from build dir): ./lms_partial_scan /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_partial_scan/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

140
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_partial_scan/src/main.cc Normal file
View File

@ -0,0 +1,140 @@
/*!
* \file main.cc
* \brief Illustrates how to acquire partial scans
* from the Sick LMS as well as telegram indices.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iomanip>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[]) {
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh
unsigned int num_values = 0; // Holds the number of measurements returned
unsigned int scan_idx = 0; // Holds the idx for the returned partial scan
unsigned int telegram_idx = 0; // Holds the idx of the telegram associated w/ scan
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_partial_scan PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_partial_scan /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate an instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Ensure real-time indices are set
*/
if (sick_lms_2xx.GetSickAvailability() & SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES) {
try {
/*
* Set the device variant to 100/0.25
*
* NOTE: Setting the variant this way ensures that the
* partial scans will start at angles that are a
* multiple of 0.25 deg.
*
*/
sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_100,SickLMS2xx::SICK_SCAN_RESOLUTION_25);
/*
* Acquire some partial scans from Sick
*/
for (unsigned int i=0; i < 12; i++) {
/*
* NOTE: Notice that here we are also obtaining the telegram idx. In a
* proper implementation, this value would be used to ensure the
* temporal consistency (acting as a sequence number) of the newly
* obtained partial scan. Here we simply just print it out.
*
* Also, we don't want Field A,B, or C outputs... so we pass in
* NULL for these params.
*/
sick_lms_2xx.GetSickPartialScan(values,num_values,scan_idx,NULL,NULL,NULL,&telegram_idx);
cout << "\t Start angle: " << setw(4) << 0.25*scan_idx << ", Num. Values: " << num_values << ", Msg Idx: " << telegram_idx << endl;
}
}
/* Catch anything else and exit */
catch(...) {
cerr << "An error occurred!" << endl;
}
}
else {
cout << "Please set the Sick LMS to an availability w/ real-time indices..." << endl;
cout << "Hint: Use the lms_config utility/example! :o)"<< endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

12
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_real_time_indices/README Normal file
View File

@ -0,0 +1,12 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_real_time_indices
Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o)
Desc: This example provides a simple program template for grabbing
data as well as real-time indices from a Sick LMS 2xx LIDAR.
Example call (from build dir): ./lms_real_time_indices /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_real_time_indices/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

125
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_real_time_indices/src/main.cc Normal file
View File

@ -0,0 +1,125 @@
/*!
* \file main.cc
* \brief Illustrates how to use the Sick Toolbox C++ interface
* to acquire scan data along with real-time indices from
* a Sick LMS 2xx.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[])
{
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh
unsigned int num_values = 0; // Holds the number of measurements returned
unsigned int telegram_idx = 0;
unsigned int real_time_idx = 0;
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_real_time_indices PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_real_time_indices /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate an instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Ensure real-time indices are enabled
*/
if (sick_lms_2xx.GetSickAvailability() & SickLMS2xx::SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES) {
try {
/*
* Acquire a few scans from the Sick LMS
*/
for (unsigned int i=0; i < 10; i++) {
/* We don't want Fields A,B, or C, so we pass NULL */
sick_lms_2xx.GetSickScan(values,num_values,NULL,NULL,NULL,&telegram_idx,&real_time_idx);
cout << "\t Num. Values: " << num_values << ", Msg Idx: " << telegram_idx
<< ", Real-time Idx: " << real_time_idx << endl;
}
}
/* Catch anything else and exit */
catch(...) {
cerr << "An error occurred!" << endl;
return -1;
}
}
else {
cout << "Please set the Sick LMS to an availability w/ real-time indices..." << endl;
cout << "Hint: Use the lms_config utility/example! :o)"<< endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

12
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_set_variant/README Normal file
View File

@ -0,0 +1,12 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_set_variant
Note: This example is NOT COMPATIBLE w/ LMS {211,221,291}-S14 LIDARs!
Desc: This example illustrates the proper way to switch the Sick LMS
variant (i.e. the Sick's FOV and scan resolution).
Example call (from build dir): ./lms_set_variant /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_set_variant/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

155
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_set_variant/src/main.cc Normal file
View File

@ -0,0 +1,155 @@
/*!
* \file main.cc
* \brief Illustrates how to set the device variant and then
* acquire measured values
*
* Note: This example WILL NOT WORK for LMS 211-S14, 221-S14,
* 291-S14 models as they do not support variant switching.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Implementation dependencies */
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
/* Use the namespace */
using namespace std;
using namespace SickToolbox;
int main(int argc, char * argv[]) {
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_set_variant PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_set_variant /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/* Instantiate the SickLMS2xx class with the device path string. */
SickLMS2xx sick_lms_2xx(device_str);
/* Define some buffers to hold the returned measurements */
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
unsigned int num_values = 0;
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Check whether device is LMS Fast
*/
if (!sick_lms_2xx.IsSickLMS2xxFast()) {
try {
/*
* Set the device variant to 100/0.25
*
* NOTE: If an invalid variant definition is
* given a SickConfigException will be
* thrown stating so.
*
*/
cout << "\tSetting variant to 100/0.25" << std::endl << flush;
sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_100,SickLMS2xx::SICK_SCAN_RESOLUTION_25);
/*
* Acquire some measurements from Sick LMS 2xx using 100/0.25
*/
cout << "\tAcquiring some measurements..." << endl;
for(unsigned int i = 0; i < 10; i++) {
/* Acquire the most recent scan from the Sick */
sick_lms_2xx.GetSickScan(values,num_values);
/* Display the number of measurements */
cout << "\t Num. Values: " << num_values << endl;
}
/*
* Set the device variant to 180/0.5
*/
cout << std::endl << "\tSetting variant to 180/0.50" << endl;
sick_lms_2xx.SetSickVariant(SickLMS2xx::SICK_SCAN_ANGLE_180,SickLMS2xx::SICK_SCAN_RESOLUTION_50);
/*
* Acquire some measurements from Sick LMS 2xx using 180/0.50
*/
cout << "\tAcquiring some measurements..." << endl;
for(unsigned int i = 0; i < 10; i++) {
/* Acquire the most recent scan from the Sick */
sick_lms_2xx.GetSickScan(values,num_values);
/* Display the number of measured values */
cout << "\t Num. Values: " << num_values << endl;
}
}
catch(...) {
cerr << "An error occurred!" << endl;
}
}
else {
cerr << "Oops... Your Sick is an LMS Fast!" << endl;
cerr << "It doesn't support the variant command." << endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

13
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_simple_app/README Normal file
View File

@ -0,0 +1,13 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_simple_app
Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o)
Desc: This example provides a simple program template for grabbing
data from a Sick LMS 2xx LIDAR. It illustrates the simplest
use of the Sick LIDAR Toolkit w/ C++.
Example call (from build dir): ./lms_simple_app /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_simple_app/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

104
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_simple_app/src/main.cc Normal file
View File

@ -0,0 +1,104 @@
/*!
* \file main.cc
* \brief A simple application using the Sick LMS 2xx driver.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[])
{
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh
unsigned int num_values = 0; // Holds the number of measurements returned
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_simple_app PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_simple_app /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate an instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Acquire a few scans from the Sick LMS
*/
try {
for (unsigned int i=0; i < 10; i++) {
sick_lms_2xx.GetSickScan(values,num_values);
cout << "\t Num. Values: " << num_values << endl;
}
}
/* Catch anything else and exit */
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

12
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_stream_range_and_reflect/README Normal file
View File

@ -0,0 +1,12 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_stream_range_and_reflect
Note: This example is COMPATIBLE w/ ONLY Sick 291-S14 (LMS FAST) LIDAR units!
Desc: This example provides a simple program template for streaming
range and reflectivity data from a Sick LMS 291-S14 LIDAR.
Example call (from build dir): ./lms_stream_range_and_reflect /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

117
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_stream_range_and_reflect/src/main.cc Normal file
View File

@ -0,0 +1,117 @@
/*!
* \file main.cc
* \brief A simple program illustrating how to stream range
* and reflectivity returns from a Sick LMS 291-S14.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[]) {
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int num_range_values;
unsigned int num_reflect_values;
unsigned int range_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
unsigned int reflect_values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0};
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (argc == 2 && strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_stream_range_and_reflect PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_stream_range_and_reflect /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate driver instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
/*
* Ensure it is an LMS Fast model
*/
if (sick_lms_2xx.IsSickLMS2xxFast()) {
/*
* Grab range and reflectivity data from the Sick LMS Fast
*/
try {
for (unsigned int i=0; i < 10; i++) {
sick_lms_2xx.GetSickScan(range_values,reflect_values,num_range_values,num_reflect_values);
cout << "Num. Range Vals: " << num_range_values << " Num. Reflect Vals: " << num_reflect_values << endl;
}
}
catch (...) {
cerr << "An error occurred!" << endl;
}
}
else {
cerr << "Oops... Your Sick is NOT an LMS Fast!" << endl;
cerr << "It doesn't support this kind of stream." << endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

12
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_subrange/README Normal file
View File

@ -0,0 +1,12 @@
=================================================
Sick LIDAR Matlab/C++ Toolbox
=================================================
Example: lms_subrange
Note: This example is COMPATIBLE w/ ALL Sick 2xx LIDAR units! :o)
Desc: This example illustrates how to acquire a scan subrange from
the Sick LMS.
Example call (from build dir): ./lms_subrange /dev/ttyUSB0 9600

1
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_subrange/src/.deps/main.Po Normal file
View File

@ -0,0 +1 @@
# dummy

109
sitl_config/ugv/sicktoolbox/c++/examples/lms2xx/lms2xx_subrange/src/main.cc Normal file
View File

@ -0,0 +1,109 @@
/*!
* \file main.cc
* \brief A simple program illustrating how to acquire a scan
* subrange from a Sick LMS 2xx.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#include <stdlib.h>
#include <string>
#include <iostream>
#include <sicktoolbox/SickLMS2xx.hh>
using namespace std;
using namespace SickToolbox;
int main(int argc, char* argv[])
{
string device_str;
SickLMS2xx::sick_lms_2xx_baud_t desired_baud = SickLMS2xx::SICK_BAUD_38400;
unsigned int values[SickLMS2xx::SICK_MAX_NUM_MEASUREMENTS] = {0}; // Uses macro defined in SickLMS2xx.hh
unsigned int num_values; // Holds the number of measurements returned
/* Check for a device path. If it's not present, print a usage statement. */
if ((argc != 2 && argc != 3) || (strcasecmp(argv[1],"--help") == 0)) {
cout << "Usage: lms2xx_subrange PATH [BAUD RATE]" << endl
<< "Ex: lms2xx_subrange /dev/ttyUSB0 9600" << endl;
return -1;
}
/* Only device path is given */
if (argc == 2) {
device_str = argv[1];
}
/* Device path and baud are given */
if (argc == 3) {
device_str = argv[1];
if ((desired_baud = SickLMS2xx::StringToSickBaud(argv[2])) == SickLMS2xx::SICK_BAUD_UNKNOWN) {
cerr << "Invalid baud value! Valid values are: 9600, 19200, 38400, and 500000" << endl;
return -1;
}
}
/*
* Instantiate an instance
*/
SickLMS2xx sick_lms_2xx(device_str);
/*
* Initialize the Sick LMS 2xx
*/
try {
sick_lms_2xx.Initialize(desired_baud);
}
catch(...) {
cerr << "Initialize failed! Are you using the correct device path?" << endl;
return -1;
}
try {
/*
* Acquire a few scans from the Sick LMS
*/
for (unsigned int i=0; i < 10; i++) {
/*
* Acquire the first ten measurements of a scan from the Sick
*/
sick_lms_2xx.GetSickScanSubrange(1,10,values,num_values);
cout << "\t Num. Values: " << num_values << endl;
}
}
catch(...) {
cerr << "An error occurred!" << endl;
}
/*
* Uninitialize the device
*/
try {
sick_lms_2xx.Uninitialize();
}
catch(...) {
cerr << "Uninitialize failed!" << endl;
return -1;
}
/* Success! */
return 0;
}

241
sitl_config/ugv/sicktoolbox/doxygen.cfg Normal file
View File

@ -0,0 +1,241 @@
# Doxygen 1.5.1
#---------------------------------------------------------------------------
# Project related configuration options
#---------------------------------------------------------------------------
PROJECT_NAME = $(PROJECT)-$(VERSION)
PROJECT_NUMBER =
OUTPUT_DIRECTORY = $(DOCDIR)
CREATE_SUBDIRS = NO
OUTPUT_LANGUAGE = English
USE_WINDOWS_ENCODING = NO
BRIEF_MEMBER_DESC = YES
REPEAT_BRIEF = YES
ABBREVIATE_BRIEF =
ALWAYS_DETAILED_SEC = NO
INLINE_INHERITED_MEMB = NO
FULL_PATH_NAMES = YES
STRIP_FROM_PATH = $(SRCDIR)
STRIP_FROM_INC_PATH = $(SRCDIR)
SHORT_NAMES = YES
JAVADOC_AUTOBRIEF = NO
MULTILINE_CPP_IS_BRIEF = NO
DETAILS_AT_TOP = NO
INHERIT_DOCS = YES
SEPARATE_MEMBER_PAGES = NO
TAB_SIZE = 8
ALIASES =
OPTIMIZE_OUTPUT_FOR_C = NO
OPTIMIZE_OUTPUT_JAVA = NO
BUILTIN_STL_SUPPORT = NO
DISTRIBUTE_GROUP_DOC = NO
SUBGROUPING = YES
#---------------------------------------------------------------------------
# Build related configuration options
#---------------------------------------------------------------------------
EXTRACT_ALL = YES
EXTRACT_PRIVATE = NO
EXTRACT_STATIC = NO
EXTRACT_LOCAL_CLASSES = YES
EXTRACT_LOCAL_METHODS = NO
HIDE_UNDOC_MEMBERS = NO
HIDE_UNDOC_CLASSES = NO
HIDE_FRIEND_COMPOUNDS = NO
HIDE_IN_BODY_DOCS = NO
INTERNAL_DOCS = NO
CASE_SENSE_NAMES = YES
HIDE_SCOPE_NAMES = NO
SHOW_INCLUDE_FILES = YES
INLINE_INFO = YES
SORT_MEMBER_DOCS = YES
SORT_BRIEF_DOCS = NO
SORT_BY_SCOPE_NAME = NO
GENERATE_TODOLIST = YES
GENERATE_TESTLIST = YES
GENERATE_BUGLIST = YES
GENERATE_DEPRECATEDLIST= YES
ENABLED_SECTIONS =
MAX_INITIALIZER_LINES = 30
SHOW_USED_FILES = YES
SHOW_DIRECTORIES = NO
FILE_VERSION_FILTER =
#---------------------------------------------------------------------------
# configuration options related to warning and progress messages
#---------------------------------------------------------------------------
QUIET = NO
WARNINGS = YES
WARN_IF_UNDOCUMENTED = YES
WARN_IF_DOC_ERROR = YES
WARN_NO_PARAMDOC = NO
WARN_FORMAT = "$file:$line: $text"
WARN_LOGFILE =
#---------------------------------------------------------------------------
# configuration options related to the input files
#---------------------------------------------------------------------------
INPUT = $(SRCDIR)
FILE_PATTERNS = *.cc *.hh
RECURSIVE = YES
EXCLUDE = $(SRCDIR)/c++/examples
EXCLUDE_SYMLINKS = NO
EXCLUDE_PATTERNS =
EXAMPLE_PATH =
EXAMPLE_PATTERNS =
EXAMPLE_RECURSIVE = NO
IMAGE_PATH =
INPUT_FILTER =
FILTER_PATTERNS =
FILTER_SOURCE_FILES = NO
#---------------------------------------------------------------------------
# configuration options related to source browsing
#---------------------------------------------------------------------------
SOURCE_BROWSER = NO
INLINE_SOURCES = NO
STRIP_CODE_COMMENTS = NO
REFERENCED_BY_RELATION = YES
REFERENCES_RELATION = YES
REFERENCES_LINK_SOURCE = YES
USE_HTAGS = NO
VERBATIM_HEADERS = YES
#---------------------------------------------------------------------------
# configuration options related to the alphabetical class index
#---------------------------------------------------------------------------
ALPHABETICAL_INDEX = YES
COLS_IN_ALPHA_INDEX = 5
IGNORE_PREFIX =
#---------------------------------------------------------------------------
# configuration options related to the HTML output
#---------------------------------------------------------------------------
GENERATE_HTML = $(GENERATE_HTML)
HTML_OUTPUT = html
HTML_FILE_EXTENSION = .html
HTML_HEADER =
HTML_FOOTER =
HTML_STYLESHEET =
HTML_ALIGN_MEMBERS = YES
GENERATE_HTMLHELP = $(GENERATE_CHM)
CHM_FILE = ../$(PROJECT).chm
HHC_LOCATION = $(HHC_PATH)
GENERATE_CHI = $(GENERATE_CHI)
BINARY_TOC = NO
TOC_EXPAND = NO
DISABLE_INDEX = NO
ENUM_VALUES_PER_LINE = 4
GENERATE_TREEVIEW = YES
TREEVIEW_WIDTH = 250
#---------------------------------------------------------------------------
# configuration options related to the LaTeX output
#---------------------------------------------------------------------------
GENERATE_LATEX = $(GENERATE_LATEX)
LATEX_OUTPUT = latex
LATEX_CMD_NAME = latex
MAKEINDEX_CMD_NAME = makeindex
COMPACT_LATEX = NO
PAPER_TYPE = $(PAPER_SIZE)
EXTRA_PACKAGES =
LATEX_HEADER =
PDF_HYPERLINKS = NO
USE_PDFLATEX = NO
LATEX_BATCHMODE = YES
LATEX_HIDE_INDICES = NO
#---------------------------------------------------------------------------
# configuration options related to the RTF output
#---------------------------------------------------------------------------
GENERATE_RTF = $(GENERATE_RTF)
RTF_OUTPUT = rtf
COMPACT_RTF = NO
RTF_HYPERLINKS = NO
RTF_STYLESHEET_FILE =
RTF_EXTENSIONS_FILE =
#---------------------------------------------------------------------------
# configuration options related to the man page output
#---------------------------------------------------------------------------
GENERATE_MAN = $(GENERATE_MAN)
MAN_OUTPUT = man
MAN_EXTENSION = .1
MAN_LINKS = NO
#---------------------------------------------------------------------------
# configuration options related to the XML output
#---------------------------------------------------------------------------
GENERATE_XML = $(GENERATE_XML)
XML_OUTPUT = xml
XML_SCHEMA =
XML_DTD =
XML_PROGRAMLISTING = YES
#---------------------------------------------------------------------------
# configuration options for the AutoGen Definitions output
#---------------------------------------------------------------------------
GENERATE_AUTOGEN_DEF = NO
#---------------------------------------------------------------------------
# configuration options related to the Perl module output
#---------------------------------------------------------------------------
GENERATE_PERLMOD = NO
PERLMOD_LATEX = NO
PERLMOD_PRETTY = YES
PERLMOD_MAKEVAR_PREFIX =
#---------------------------------------------------------------------------
# Configuration options related to the preprocessor
#---------------------------------------------------------------------------
ENABLE_PREPROCESSING = YES
MACRO_EXPANSION = NO
EXPAND_ONLY_PREDEF = NO
SEARCH_INCLUDES = YES
INCLUDE_PATH =
INCLUDE_FILE_PATTERNS =
PREDEFINED =
EXPAND_AS_DEFINED =
SKIP_FUNCTION_MACROS = YES
#---------------------------------------------------------------------------
# Configuration::additions related to external references
#---------------------------------------------------------------------------
TAGFILES =
GENERATE_TAGFILE = $(DOCDIR)/$(PROJECT).tag
ALLEXTERNALS = NO
EXTERNAL_GROUPS = YES
PERL_PATH = $(PERL_PATH)
#---------------------------------------------------------------------------
# Configuration options related to the dot tool
#---------------------------------------------------------------------------
CLASS_DIAGRAMS = YES
HIDE_UNDOC_RELATIONS = YES
HAVE_DOT = $(HAVE_DOT)
CLASS_GRAPH = YES
COLLABORATION_GRAPH = YES
GROUP_GRAPHS = YES
UML_LOOK = NO
TEMPLATE_RELATIONS = NO
INCLUDE_GRAPH = YES
INCLUDED_BY_GRAPH = YES
CALL_GRAPH = NO
CALLER_GRAPH = NO
GRAPHICAL_HIERARCHY = YES
DIRECTORY_GRAPH = YES
DOT_IMAGE_FORMAT = png
DOT_PATH = $(DOT_PATH)
DOTFILE_DIRS =
MAX_DOT_GRAPH_WIDTH = 1024
MAX_DOT_GRAPH_HEIGHT = 1024
MAX_DOT_GRAPH_DEPTH = 0
DOT_TRANSPARENT = NO
DOT_MULTI_TARGETS = NO
GENERATE_LEGEND = YES
DOT_CLEANUP = YES
#---------------------------------------------------------------------------
# Configuration::additions related to the search engine
#---------------------------------------------------------------------------
SEARCHENGINE = NO

471
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickBufferMonitor.hh Normal file
View File

@ -0,0 +1,471 @@
/*!
* \file SickBufferMonitor.hh
* \brief Defines an abstract class for interfacing with a Sick LIDAR.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_BUFFER_MONITOR
#define SICK_BUFFER_MONITOR
/* Dependencies */
#include <iostream>
#include <pthread.h>
#include <unistd.h>
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/**
* \class SickBufferMonitor
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
class SickBufferMonitor {
public:
/** A standard constructor */
SickBufferMonitor( SICK_MONITOR_CLASS * const monitor_instance ) throw( SickThreadException );
/** A method for setting the target data stream */
void SetDataStream( const unsigned int sick_fd ) throw( SickThreadException );
/** Start the buffer monitor for the device */
void StartMonitor( const unsigned int sick_fd ) throw( SickThreadException );
/** Acquire the most recent message buffered by the monitor */
bool GetNextMessageFromMonitor( SICK_MSG_CLASS &sick_message ) throw( SickThreadException );
/** Stop the buffer monitor for the device */
void StopMonitor( ) throw( SickThreadException );
/** Locks access to the data stream */
void AcquireDataStream( ) throw( SickThreadException );
/** Acquire the next message from raw byte stream */
void GetNextMessageFromDataStream( SICK_MSG_CLASS &sick_message );
/** Unlock access to the data stream */
void ReleaseDataStream( ) throw( SickThreadException );
/** A standard destructor */
~SickBufferMonitor( ) throw( SickThreadException );
protected:
/** Sick data stream file descriptor */
unsigned int _sick_fd;
/** Reads n bytes into the destination buffer */
void _readBytes( uint8_t * const dest_buffer, const int num_bytes_to_read, const unsigned int timeout_value = 0 ) const throw ( SickTimeoutException, SickIOException );
private:
/** The current monitor instance */
SICK_MONITOR_CLASS *_sick_monitor_instance;
/** A flag to indicate the monitor should continue running */
bool _continue_grabbing;
/** Buffer monitor thread ID */
pthread_t _monitor_thread_id;
/** A mutex for guarding the message container */
pthread_mutex_t _container_mutex;
/** A mutex for locking the data stream */
pthread_mutex_t _stream_mutex;
/** A container to hold the most recent message */
SICK_MSG_CLASS _recv_msg_container;
/** Locks access to the message container */
void _acquireMessageContainer( ) throw( SickThreadException );
/** Unlocks access to the message container */
void _releaseMessageContainer( ) throw( SickThreadException );
/** Entry point for the monitor thread */
static void * _bufferMonitorThread( void * thread_args );
};
/**
* \brief Primary constructor
* \param device_instance A pointer to the current driver instance
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickBufferMonitor( SICK_MONITOR_CLASS * const monitor_instance ) throw( SickThreadException ) :
_sick_monitor_instance(monitor_instance), _continue_grabbing(true), _monitor_thread_id(0) {
/* Initialize the shared message buffer mutex */
if (pthread_mutex_init(&_container_mutex,NULL) != 0) {
throw SickThreadException("SickBufferMonitor::SickBufferMonitor: pthread_mutex_init() failed!");
}
/* Initialize the shared data stream mutex */
if (pthread_mutex_init(&_stream_mutex,NULL) != 0) {
throw SickThreadException("SickBufferMonitor::SickBufferMonitor: pthread_mutex_init() failed!");
}
}
/**
* \brief A method for setting/changing the current data stream
* \param sick_fd The data stream file descriptor
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SetDataStream( const unsigned int sick_fd ) throw ( SickThreadException ) {
try {
/* Attempt to acquire the data stream */
AcquireDataStream();
/* Assign the data stream fd */
_sick_fd = sick_fd;
/* Attempt to release the data stream */
ReleaseDataStream();
}
/* Handle thread exception */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
}
/* A safety net */
catch(...) {
std::cerr << "SickBufferMonitor::SetDataStream: Unknown exception!" << std::endl;
throw;
}
}
/**
* \brief Creates and starts the buffer monitor thread
* \return True upon success, False otherwise
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::StartMonitor( const unsigned int sick_fd ) throw ( SickThreadException ) {
/* Assign the fd associated with the data stream */
_sick_fd = sick_fd;
/* Start the buffer monitor */
if (pthread_create(&_monitor_thread_id,NULL,SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_bufferMonitorThread,_sick_monitor_instance) != 0) {
throw SickThreadException("SickBufferMonitor::StartMonitor: pthread_create() failed!");
}
/* Set the flag to continue grabbing data */
_continue_grabbing = true;
}
/**
* \brief Checks the message container for the next available Sick message
* \param &sick_message The message object that is to be populated with the results
* \return True if the current contents were acquired, false otherwise
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
bool SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::GetNextMessageFromMonitor( SICK_MSG_CLASS &sick_message ) throw( SickThreadException ) {
bool acquired_message = false;
try {
/* Acquire a lock on the message buffer */
_acquireMessageContainer();
/* Check whether the object is populated */
if (_recv_msg_container.IsPopulated()) {
/* Copy the shared message */
sick_message = _recv_msg_container;
_recv_msg_container.Clear();
/* Set the flag indicating success */
acquired_message = true;
}
/* Release message container */
_releaseMessageContainer();
}
/* Handle a thread exception */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
throw;
}
/* Handle an unknown exception */
catch(...) {
std::cerr << "SickBufferMonitor::CheckMessageContainer: Unknown exception!" << std::endl;
throw;
}
/* Return the flag */
return acquired_message;
}
/**
* \brief Cancels the buffer monitor thread
* \return True if the thread was properly canceled, false otherwise
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::StopMonitor( ) throw ( SickThreadException ) {
try {
/* Return results from the thread */
void *monitor_result = NULL;
/* Tell the thread to quit working */
AcquireDataStream();
_continue_grabbing = false;
ReleaseDataStream();
/* Wait for the buffer monitor to exit */
if (pthread_join(_monitor_thread_id,&monitor_result) != 0) {
throw SickThreadException("SickBufferMonitor::StopMonitor: pthread_join() failed!");
}
}
/* Handle thread exception */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
}
/* A safety net */
catch(...) {
std::cerr << "SickBufferMonitor::StopMonitor: Unknown exception!" << std::endl;
throw;
}
}
/**
* \brief Acquires a lock on the data stream
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::AcquireDataStream( ) throw( SickThreadException ) {
/* Attempt to lock the stream mutex */
if (pthread_mutex_lock(&_stream_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::AcquireDataStream: pthread_mutex_lock() failed!");
}
}
/**
* \brief Releases a lock on the data stream
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::ReleaseDataStream( ) throw( SickThreadException ) {
/* Attempt to lock the stream mutex */
if (pthread_mutex_unlock(&_stream_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::ReleaseDataStream: pthread_mutex_unlock() failed!");
}
}
/**
* \brief The destructor (kills the mutex)
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickBufferMonitor( ) throw( SickThreadException ) {
/* Destroy the message container mutex */
if (pthread_mutex_destroy(&_container_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::~SickBufferMonitor: pthread_mutex_destroy() failed!");
}
/* Destroy the data stream container mutex */
if (pthread_mutex_destroy(&_stream_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::~SickBufferMonitor: pthread_mutex_destroy() failed!");
}
}
/**
* \brief Locks access to the message container
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_acquireMessageContainer( ) throw( SickThreadException ) {
/* Lock the mutex */
if (pthread_mutex_lock(&_container_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::_acquireMessageContainer: pthread_mutex_lock() failed!");
}
}
/**
* \brief Unlocks access to the message container
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_releaseMessageContainer( ) throw( SickThreadException ) {
/* Unlock the mutex */
if (pthread_mutex_unlock(&_container_mutex) != 0) {
throw SickThreadException("SickBufferMonitor::_releaseMessageContainer: pthread_mutex_unlock() failed!");
}
}
/**
* \brief Attempt to read a certain number of bytes from the stream
* \param *dest_buffer A pointer to the destination buffer
* \param num_bytes_to_read The number of bytes to read into the buffer
* \param timeout_value The number of microseconds allowed between subsequent bytes in a message
* \return True if the number of requested bytes were successfully read
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_readBytes( uint8_t * const dest_buffer, const int num_bytes_to_read, const unsigned int timeout_value ) const
throw ( SickTimeoutException, SickIOException ) {
/* Some helpful variables */
int num_bytes_read = 0;
int total_num_bytes_read = 0;
int num_active_files = 0;
struct timeval timeout_val; // This structure will be used for setting our timeout values
fd_set file_desc_set; // File descriptor set for monitoring I/O
/* Attempt to fetch the bytes */
while ( total_num_bytes_read < num_bytes_to_read ) {
/* Initialize and set the file descriptor set for select */
FD_ZERO(&file_desc_set);
FD_SET(_sick_fd,&file_desc_set);
/* Setup the timeout structure */
memset(&timeout_val,0,sizeof(timeout_val)); // Initialize the buffer
timeout_val.tv_usec = timeout_value; // Wait for specified time before throwing a timeout
/* Wait for the OS to tell us that data is waiting! */
num_active_files = select(getdtablesize(),&file_desc_set,0,0,(timeout_value > 0) ? &timeout_val : 0);
/* Figure out what to do based on the output of select */
if (num_active_files > 0) {
/* A file is ready for reading!
*
* NOTE: The following conditional is just a sanity check. Since
* the file descriptor set only contains the sick device fd,
* it likely unnecessary to use FD_ISSET
*/
if (FD_ISSET(_sick_fd,&file_desc_set)) {
/* Read a single byte from the stream! */
num_bytes_read = read(_sick_fd,&dest_buffer[total_num_bytes_read],1);
/* Decide what to do based on the output of read */
if (num_bytes_read > 0) { //Update the number of bytes read so far
total_num_bytes_read += num_bytes_read;
}
else {
/* If this happens, something is wrong */
throw SickIOException("SickBufferMonitor::_readBytes: read() failed!");
}
}
}
else if (num_active_files == 0) {
/* A timeout has occurred! */
throw SickTimeoutException("SickBufferMonitor::_readBytes: select() timeout!");
}
else {
/* An error has occurred! */
throw SickIOException("SickBufferMonitor::_readBytes: select() failed!");
}
}
}
/**
* \brief The monitor thread
* \param *args The thread arguments
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void * SickBufferMonitor< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_bufferMonitorThread( void * thread_args ) {
/* Declare a Sick LD receive object */
SICK_MSG_CLASS curr_message;
/* Acquire the Sick device instance */
SICK_MONITOR_CLASS *buffer_monitor = (SICK_MONITOR_CLASS *)thread_args;
/* The main thread control loop */
for (;;) {
try {
/* Reset the sick message object */
curr_message.Clear();
/* Acquire the most recent message */
buffer_monitor->AcquireDataStream();
if (!buffer_monitor->_continue_grabbing) { // should the thread continue grabbing
buffer_monitor->ReleaseDataStream();
break;
}
buffer_monitor->GetNextMessageFromDataStream(curr_message);
buffer_monitor->ReleaseDataStream();
/* Update message container contents */
buffer_monitor->_acquireMessageContainer();
buffer_monitor->_recv_msg_container = curr_message;
buffer_monitor->_releaseMessageContainer();
}
/* Make sure there wasn't a serious error reading from the buffer */
catch(SickIOException &sick_io_exception) {
std::cerr << sick_io_exception.what() << std::endl;
}
/* Catch any thread exceptions */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
}
/* A failsafe */
catch(...) {
std::cerr << "SickBufferMonitor::_bufferMonitorThread: Unknown exception!" << std::endl;
}
/* sleep a bit! */
usleep(1000);
}
/* Thread is done */
return NULL;
}
} /* namespace SickToolbox */
#endif /* SICK_BUFFER_MONITOR */

186
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickConfig.hh Normal file
View File

@ -0,0 +1,186 @@
/* c++/drivers/base/src/SickConfig.hh. Generated from SickConfig.hh.in by configure. */
/* c++/drivers/base/src/SickConfig.hh.in. Generated from configure.ac by autoheader. */
/* Define if building universal (internal helper macro) */
/* #undef AC_APPLE_UNIVERSAL_BUILD */
/* Define to 1 if you have the <arpa/inet.h> header file. */
#define HAVE_ARPA_INET_H 1
/* Define to 1 if you have the <dlfcn.h> header file. */
#define HAVE_DLFCN_H 1
/* Define to 1 if you have the <fcntl.h> header file. */
#define HAVE_FCNTL_H 1
/* Define to 1 if you have the `gettimeofday' function. */
#define HAVE_GETTIMEOFDAY 1
/* Define if you have gnuplot */
/* #undef HAVE_GNUPLOT */
/* Define to 1 if you have the <inttypes.h> header file. */
#define HAVE_INTTYPES_H 1
/* Define if you have linux/serial.h */
#define HAVE_LINUX_SERIAL_H 1
/* Define to 1 if you have the <memory.h> header file. */
#define HAVE_MEMORY_H 1
/* Define to 1 if you have the `memset' function. */
#define HAVE_MEMSET 1
/* Define to 1 if you have the <netinet/in.h> header file. */
#define HAVE_NETINET_IN_H 1
/* Define if you have POSIX threads libraries and header files. */
#define HAVE_PTHREAD 1
/* Define to 1 if you have the `select' function. */
#define HAVE_SELECT 1
/* Define to 1 if you have the `socket' function. */
#define HAVE_SOCKET 1
/* Define to 1 if stdbool.h conforms to C99. */
#define HAVE_STDBOOL_H 1
/* Define to 1 if you have the <stdint.h> header file. */
#define HAVE_STDINT_H 1
/* Define to 1 if you have the <stdlib.h> header file. */
#define HAVE_STDLIB_H 1
/* Define to 1 if you have the `strcasecmp' function. */
#define HAVE_STRCASECMP 1
/* Define to 1 if you have the <strings.h> header file. */
#define HAVE_STRINGS_H 1
/* Define to 1 if you have the <string.h> header file. */
#define HAVE_STRING_H 1
/* Define to 1 if you have the <sys/ioctl.h> header file. */
#define HAVE_SYS_IOCTL_H 1
/* Define to 1 if you have the <sys/select.h> header file. */
#define HAVE_SYS_SELECT_H 1
/* Define to 1 if you have the <sys/socket.h> header file. */
#define HAVE_SYS_SOCKET_H 1
/* Define to 1 if you have the <sys/stat.h> header file. */
#define HAVE_SYS_STAT_H 1
/* Define to 1 if you have the <sys/time.h> header file. */
#define HAVE_SYS_TIME_H 1
/* Define to 1 if you have the <sys/types.h> header file. */
#define HAVE_SYS_TYPES_H 1
/* Define to 1 if you have the <termios.h> header file. */
#define HAVE_TERMIOS_H 1
/* Define to 1 if you have the <unistd.h> header file. */
#define HAVE_UNISTD_H 1
/* Define to 1 if the system has the type `_Bool'. */
#define HAVE__BOOL 1
/* Define to the sub-directory in which libtool stores uninstalled libraries.
*/
#define LT_OBJDIR ".libs/"
/* Name of package */
#define PACKAGE "sicktoolbox"
/* Define to the address where bug reports for this package should be sent. */
#define PACKAGE_BUGREPORT "jasonder@seas.upenn.edu"
/* Define to the full name of this package. */
#define PACKAGE_NAME "SickToolbox"
/* Define to the full name and version of this package. */
#define PACKAGE_STRING "SickToolbox 1.1"
/* Define to the one symbol short name of this package. */
#define PACKAGE_TARNAME "sicktoolbox"
/* Define to the home page for this package. */
#define PACKAGE_URL ""
/* Define to the version of this package. */
#define PACKAGE_VERSION "1.1"
/* Define to necessary symbol if this constant uses a non-standard name on
your system. */
/* #undef PTHREAD_CREATE_JOINABLE */
/* Define as the return type of signal handlers (`int' or `void'). */
#define RETSIGTYPE void
/* Define to the type of arg 1 for `select'. */
#define SELECT_TYPE_ARG1 int
/* Define to the type of args 2, 3 and 4 for `select'. */
#define SELECT_TYPE_ARG234 (fd_set *)
/* Define to the type of arg 5 for `select'. */
#define SELECT_TYPE_ARG5 (struct timeval *)
/* Define to 1 if you have the ANSI C header files. */
#define STDC_HEADERS 1
/* Define to 1 if you can safely include both <sys/time.h> and <time.h>. */
#define TIME_WITH_SYS_TIME 1
/* Version number of package */
#define VERSION "1.1"
/* Define WORDS_BIGENDIAN to 1 if your processor stores words with the most
significant byte first (like Motorola and SPARC, unlike Intel). */
#if defined AC_APPLE_UNIVERSAL_BUILD
# if defined __BIG_ENDIAN__
# define WORDS_BIGENDIAN 1
# endif
#else
# ifndef WORDS_BIGENDIAN
/* # undef WORDS_BIGENDIAN */
# endif
#endif
/* Define for Solaris 2.5.1 so the uint32_t typedef from <sys/synch.h>,
<pthread.h>, or <semaphore.h> is not used. If the typedef were allowed, the
#define below would cause a syntax error. */
/* #undef _UINT32_T */
/* Define for Solaris 2.5.1 so the uint8_t typedef from <sys/synch.h>,
<pthread.h>, or <semaphore.h> is not used. If the typedef were allowed, the
#define below would cause a syntax error. */
/* #undef _UINT8_T */
/* Define to empty if `const' does not conform to ANSI C. */
/* #undef const */
/* Define to `__inline__' or `__inline' if that's what the C compiler
calls it, or to nothing if 'inline' is not supported under any name. */
#ifndef __cplusplus
/* #undef inline */
#endif
/* Define to the type of a signed integer type of width exactly 16 bits if
such a type exists and the standard includes do not define it. */
/* #undef int16_t */
/* Define to the type of an unsigned integer type of width exactly 16 bits if
such a type exists and the standard includes do not define it. */
/* #undef uint16_t */
/* Define to the type of an unsigned integer type of width exactly 32 bits if
such a type exists and the standard includes do not define it. */
/* #undef uint32_t */
/* Define to the type of an unsigned integer type of width exactly 8 bits if
such a type exists and the standard includes do not define it. */
/* #undef uint8_t */

244
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickException.hh Normal file
View File

@ -0,0 +1,244 @@
/*!
* \file SickException.hh
* \brief Contains some simple exception classes.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_EXCEPTION
#define SICK_EXCEPTION
/* Definition dependencies */
#include <string>
#include <exception>
/* Associate the namespace */
namespace SickToolbox {
/** \class SickException
* \brief Provides a base exception class from
* which to derive other Sick exceptions
*/
class SickException : std::exception {
public:
/**
* \brief A standard constructor
* \param general_str A descriptive "general" string
*/
SickException( const std::string general_str ) {
_detailed_msg = general_str;
}
/**
* \brief A standard constructor
* \param general_str A descriptive "general" string
* \param detailed_str A more detailed description
*/
SickException( const std::string general_str, const std::string detailed_str ) {
_detailed_msg = general_str + " " + detailed_str;
}
/**
* \brief From the standard exception library
*/
virtual const char* what( ) const throw() {
return _detailed_msg.c_str();
}
/**
* \brief A destructor
*/
~SickException() throw() {}
private:
/** The string identifier */
std::string _detailed_msg;
};
/**
* \class SickTimeoutException
* \brief Makes handling timeouts much easier
*/
class SickTimeoutException : public SickException {
public:
/**
* \brief A constructor
*/
SickTimeoutException() :
SickException("A Timeout Occurred!") { }
/**
* \brief A constructor
* \param detailed_str A more detailed description
*/
SickTimeoutException( const std::string detailed_str ) :
SickException("A Timeout Occurred -",detailed_str) { }
/**
* \brief A destructor
*/
~SickTimeoutException() throw() { }
};
/**
* \class SickIOException
* \brief Thrown instance where the driver can't
* read,write,drain,flush,... the buffers
*/
class SickIOException : public SickException {
public:
/**
* \brief A constructor
*/
SickIOException() :
SickException("ERROR: I/O exception!") { }
/**
* \brief Another constructor
* \param detailed_str A more detailed description
*/
SickIOException( const std::string detailed_str ) :
SickException("ERROR: I/O exception -",detailed_str) { }
/**
* \brief A destructor
*/
~SickIOException() throw() { }
};
/**
* \class SickBadChecksumException
* \brief Thrown when a received message has an
* invalid checksum
*/
class SickBadChecksumException : public SickException {
public:
/**
* \brief A constructor
*/
SickBadChecksumException() :
SickException("ERROR: Bad Checksum!") { }
/**
* \brief Another constructor
* \param detailed_str A more detailed description
*/
SickBadChecksumException( const std::string detailed_str ) :
SickException("ERROR: Bad Checksum -",detailed_str) { }
/**
* \brief A destructor
*/
~SickBadChecksumException() throw() { }
};
/**
* \class SickThreadException
* \brief Thrown when error occurs during thread
* initialization, and uninitialization
*/
class SickThreadException : public SickException {
public:
/**
* \brief A constructor
*/
SickThreadException() :
SickException("ERROR: Sick thread exception!") { }
/**
* \brief Another constructor
* \param detailed_str A more detailed description
*/
SickThreadException( const std::string detailed_str ) :
SickException("ERROR: Sick thread exception -",detailed_str) { }
/**
* \brief A destructor
*/
~SickThreadException() throw() { }
};
/**
* \class SickConfigException
* \brief Thrown when the driver detects (or the Sick reports)
* an invalid config
*/
class SickConfigException : public SickException {
public:
/**
* \brief A constructor
*/
SickConfigException() :
SickException("ERROR: Config exception!") { }
/**
* \brief Another constructor
* \param detailed_str A more detailed description
*/
SickConfigException( const std::string detailed_str ) :
SickException("ERROR: Config exception -",detailed_str) { }
/**
* \brief A destructor
*/
~SickConfigException() throw() { }
};
/**
* \class SickErrorException
* \brief Thrown when Sick returns an error code
* or an unexpected response.
*/
class SickErrorException : public SickException {
public:
/**
* \brief A constructor
*/
SickErrorException() :
SickException("ERROR: Sick returned error code!") { };
/**
* \brief Another constructor
* \param detailed_str A more detailed description
*/
SickErrorException( const std::string detailed_str ) :
SickException("ERROR: Sick error -", detailed_str) { }
/**
* \brief A destructor
*/
~SickErrorException() throw() { }
};
} /* namespace SickToolbox */
#endif /* SICK_EXCEPTION */

805
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLD.hh Normal file
View File

@ -0,0 +1,805 @@
/*!
* \file SickLD.hh
* \brief Defines the SickLD class for working with the
* Sick LD-OEM/LD-LRS long range LIDARs.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LD_HH
#define SICK_LD_HH
/* Macros */
#define DEFAULT_SICK_IP_ADDRESS "192.168.1.10" ///< Default Sick LD INet 4 address
#define DEFAULT_SICK_TCP_PORT (49152) ///< Default TCP port
#define DEFAULT_SICK_MESSAGE_TIMEOUT (unsigned int)(5e6) ///< The max time to wait for a message reply (usecs)
#define DEFAULT_SICK_CONNECT_TIMEOUT (unsigned int)(1e6) ///< The max time to wait before considering a connection attempt as failed (usecs)
#define DEFAULT_SICK_NUM_SCAN_PROFILES (0) ///< Setting this value to 0 will tell the Sick LD to stream measurements when measurement data is requested (NOTE: A profile is a single scans worth of range measurements)
#define DEFAULT_SICK_SIGNAL_SET (0) ///< Default Sick signal configuration
/**
* \def SWAP_VALUES(x,y,t)
* \brief A simple macro for swapping two values.
*/
#define SWAP_VALUES(x,y,t) (t=x,x=y,y=t);
/* Definition dependencies */
#include <string>
#include <vector>
#include <pthread.h>
#include <arpa/inet.h>
#include "SickLIDAR.hh"
#include "SickLDBufferMonitor.hh"
#include "SickLDMessage.hh"
#include "SickException.hh"
/**
* \namespace SickToolbox
* \brief Encapsulates the Sick LIDAR Matlab/C++ toolbox
*/
namespace SickToolbox {
/**
* \class SickLD
* \brief Provides a simple driver interface for working with the
* Sick LD-OEM/Sick LD-LRS long-range models via Ethernet.
*/
class SickLD : public SickLIDAR< SickLDBufferMonitor, SickLDMessage > {
public:
/* Some constants for the developer/end-user */
static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 2881; ///< Maximum number of measurements per sector
static const uint16_t SICK_MAX_NUM_SECTORS = 8; ///< Maximum number of scan sectors (NOTE: This value must be even)
static const uint16_t SICK_MAX_NUM_MEASURING_SECTORS = 4; ///< Maximum number of active/measuring scan sectors
static const uint16_t SICK_MAX_SCAN_AREA = 360; ///< Maximum area that can be covered in a single scan (deg)
static const uint16_t SICK_MIN_MOTOR_SPEED = 5; ///< Minimum motor speed in Hz
static const uint16_t SICK_MAX_MOTOR_SPEED = 20; ///< Maximum motor speed in Hz
static const uint16_t SICK_MIN_VALID_SENSOR_ID = 1; ///< The lowest value the Sick will accept as a Sensor ID
static const uint16_t SICK_MAX_VALID_SENSOR_ID = 254; ///< The largest value the Sick will accept as a Sensor ID
static const uint16_t SICK_MAX_MEAN_PULSE_FREQUENCY = 10800; ///< Max mean pulse frequence of the current device configuration (in Hz) (see page 22 of the operator's manual)
static const uint16_t SICK_MAX_PULSE_FREQUENCY = 14400; ///< Max pulse frequency of the device (in Hz) (see page 22 of the operator's manual)
static const uint16_t SICK_NUM_TICKS_PER_MOTOR_REV = 5760; ///< Odometer ticks per revolution of the Sick LD scan head
static constexpr double SICK_MAX_SCAN_ANGULAR_RESOLUTION = 0.125; ///< Minimum valid separation between laser pulses in active scan ares (deg)
static constexpr double SICK_DEGREES_PER_MOTOR_STEP = 0.0625; ///< Each odometer tick is equivalent to rotating the scan head this many degrees
/* Sick LD sensor modes of operation */
static const uint8_t SICK_SENSOR_MODE_IDLE = 0x01; ///< The Sick LD is powered but idle
static const uint8_t SICK_SENSOR_MODE_ROTATE = 0x02; ///< The Sick LD prism is rotating, but laser is off
static const uint8_t SICK_SENSOR_MODE_MEASURE = 0x03; ///< The Sick LD prism is rotating, and the laser is on
static const uint8_t SICK_SENSOR_MODE_ERROR = 0x04; ///< The Sick LD is in error mode
static const uint8_t SICK_SENSOR_MODE_UNKNOWN = 0xFF; ///< The Sick LD is in an unknown state
/* Sick LD motor modes */
static const uint8_t SICK_MOTOR_MODE_OK = 0x00; ///< Motor is functioning properly
static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_HIGH = 0x09; ///< Motor spin too low (i.e. rotational velocity too low)
static const uint8_t SICK_MOTOR_MODE_SPIN_TOO_LOW = 0x04; ///< Motor spin too high (i.e. rotational velocity too fast)
static const uint8_t SICK_MOTOR_MODE_ERROR = 0x0B; ///< Motor stops or coder error
static const uint8_t SICK_MOTOR_MODE_UNKNOWN = 0xFF; ///< Motor is in an unknown state
/* Sick LD service codes */
static const uint8_t SICK_STAT_SERV_CODE = 0x01; ///< Status service code
static const uint8_t SICK_CONF_SERV_CODE = 0x02; ///< Configuration service code
static const uint8_t SICK_MEAS_SERV_CODE = 0x03; ///< Measurement service code
static const uint8_t SICK_WORK_SERV_CODE = 0x04; ///< Working service code
static const uint8_t SICK_ROUT_SERV_CODE = 0x06; ///< Routing service code
static const uint8_t SICK_FILE_SERV_CODE = 0x07; ///< File service code
static const uint8_t SICK_MONR_SERV_CODE = 0x08; ///< Monitor service code
/* Sick LD status services (service code 0x01) */
static const uint8_t SICK_STAT_SERV_GET_ID = 0x01; ///< Request the Sick LD ID
static const uint8_t SICK_STAT_SERV_GET_STATUS = 0x02; ///< Request status information
static const uint8_t SICK_STAT_SERV_GET_SIGNAL = 0x04; ///< Reads the value of the switch and LED port
static const uint8_t SICK_STAT_SERV_SET_SIGNAL = 0x05; ///< Sets the switches and LEDs
static const uint8_t SICK_STAT_SERV_LD_REGISTER_APPLICATION = 0x06; ///< Registers the ID data for the application firmware
/* Sick LD status service GET_IDENTIFICATION request codes */
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_PART_NUM = 0x00; ///< Request the sensor's part number
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_NAME = 0x01; ///< Request the sensor's name
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_VERSION = 0x02; ///< Request the sensor's version
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_SERIAL_NUM = 0x03; ///< Request the sensor's serial number
static const uint8_t SICK_STAT_SERV_GET_ID_SENSOR_EDM_SERIAL_NUM = 0x04; ///< Request the edm??? serial number
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_PART_NUM = 0x10; ///< Requess the firmware's part number
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_NAME = 0x11; ///< Request the firmware's name
static const uint8_t SICK_STAT_SERV_GET_ID_FIRMWARE_VERSION = 0x12; ///< Request the firmware's version
static const uint8_t SICK_STAT_SERV_GET_ID_APP_PART_NUM = 0x20; ///< Request the application part number
static const uint8_t SICK_STAT_SERV_GET_ID_APP_NAME = 0x21; ///< Request the application name
static const uint8_t SICK_STAT_SERV_GET_ID_APP_VERSION = 0x22; ///< Request the application version
/* Sick LD configuration services (service code 0x02) */
static const uint8_t SICK_CONF_SERV_SET_CONFIGURATION = 0x01; ///< Set the Sick LD configuration
static const uint8_t SICK_CONF_SERV_GET_CONFIGURATION = 0x02; ///< Read the Sick LD configuration information
static const uint8_t SICK_CONF_SERV_SET_TIME_ABSOLUTE = 0x03; ///< Set the internal clock to a timestamp value
static const uint8_t SICK_CONF_SERV_SET_TIME_RELATIVE = 0x04; ///< Correct the internal clock by some value
static const uint8_t SICK_CONF_SERV_GET_SYNC_CLOCK = 0x05; ///< Read the internal time of the LD-OEM/LD-LRS
static const uint8_t SICK_CONF_SERV_SET_FILTER = 0x09; ///< Set the filter configuration
static const uint8_t SICK_CONF_SERV_SET_FUNCTION = 0x0A; ///< Assigns a measurement function to an angle range
static const uint8_t SICK_CONF_SERV_GET_FUNCTION = 0x0B; ///< Returns the configuration of the given sector
/* Sick LD configuration filter codes */
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD = 0x01; ///< Code for identifying filter type: nearfield suppression
/* Sick LD nearfield suppression configuration codes */
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_OFF = 0x00; ///< Used to set nearfield suppression off
static const uint8_t SICK_CONF_SERV_SET_FILTER_NEARFIELD_ON = 0x01; ///< Used to set nearfield suppression on
/* Sick LD measurement services (service code 0x03) */
static const uint8_t SICK_MEAS_SERV_GET_PROFILE = 0x01; ///< Requests n profiles of a defined format
static const uint8_t SICK_MEAS_SERV_CANCEL_PROFILE = 0x02; ///< Stops profile output
/* Sick LD working services (service code 0x04) */
static const uint8_t SICK_WORK_SERV_RESET = 0x01; ///< Sick LD enters a reset sequence
static const uint8_t SICK_WORK_SERV_TRANS_IDLE = 0x02; ///< Sick LD enters IDLE mode (motor stops and laser is turned off)
static const uint8_t SICK_WORK_SERV_TRANS_ROTATE = 0x03; ///< Sick LD enters ROTATE mode (motor starts and rotates with a specified speed in Hz, laser is off)
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE = 0x04; ///< Sick LD enters MEASURE mode (laser starts with next revolution)
/* Sick LD working service DO_RESET request codes */
static const uint8_t SICK_WORK_SERV_RESET_INIT_CPU = 0x00; ///< Sick LD does a complete reset (Reinitializes the CPU)
static const uint8_t SICK_WORK_SERV_RESET_KEEP_CPU = 0x01; ///< Sick LD does a partial reset (CPU is not reinitialized)
static const uint8_t SICK_WORK_SERV_RESET_HALT_APP = 0x02; ///< Sick LD does a minimal reset (Application is halted and device enters IDLE state)
/* Sick LD working service TRANS_MEASURE return codes */
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_OK = 0x00; ///< Sick LD is ready to stream/obtain scan profiles
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MAX_PULSE = 0x01; ///< Sick LD reports config yields a max laser pulse frequency that is too high
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_MEAN_PULSE = 0x02; ///< Sick LD reports config yields a max mean pulse frequency that is too high
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER = 0x03; ///< Sick LD reports sector borders are not configured correctly
static const uint8_t SICK_WORK_SERV_TRANS_MEASURE_RET_ERR_SECT_BORDER_MULT = 0x04; ///< Sick LD reports sector borders are not a multiple of the step angle
/* Sick LD interface routing services (service code 0x06) */
static const uint8_t SICK_ROUT_SERV_COM_ATTACH = 0x01; ///< Attach a master (host) communications interface
static const uint8_t SICK_ROUT_SERV_COM_DETACH = 0x02; ///< Detach a master (host) communications interface
static const uint8_t SICK_ROUT_SERV_COM_INITIALIZE = 0x03; ///< Initialize the interface (Note: using this may not be necessary for some interfaces, e.g. Ethernet)
static const uint8_t SICK_ROUT_SERV_COM_OUTPUT = 0x04; ///< Output data to the interface
static const uint8_t SICK_ROUT_SERV_COM_DATA = 0x05; ///< Forward data received on specified interface to master interface
/* Sick LD file services (service code 0x07) */
static const uint8_t SICK_FILE_SERV_DIR = 0x01; ///< List the stored files in flash memory
static const uint8_t SICK_FILE_SERV_SAVE = 0x02; ///< Saves the data into flash memory
static const uint8_t SICK_FILE_SERV_LOAD = 0x03; ///< Recalls a file from the flash
static const uint8_t SICK_FILE_SERV_DELETE = 0x04; ///< Deletes a file from the flash
/* Sick LD monitor services (service code 0x08) */
static const uint8_t SICK_MONR_SERV_MONITOR_RUN = 0x01; ///< Enable/disable monitor services
static const uint8_t SICK_MONR_SERV_MONITOR_PROFILE_LOG = 0x02; ///< Enable/disable profile logging
/* Sick LD configuration keys */
static const uint8_t SICK_CONF_KEY_RS232_RS422 = 0x01; ///< Key for configuring RS-232/RS-422
static const uint8_t SICK_CONF_KEY_CAN = 0x02; ///< Key for configuring CAN
static const uint8_t SICK_CONF_KEY_ETHERNET = 0x05; ///< Key for configuring Ethernet
static const uint8_t SICK_CONF_KEY_GLOBAL = 0x10; ///< Key for global configuration
/* Sick LD sector configuration codes */
static const uint8_t SICK_CONF_SECTOR_NOT_INITIALIZED = 0x00; ///< Sector is uninitialized
static const uint8_t SICK_CONF_SECTOR_NO_MEASUREMENT = 0x01; ///< Sector has no measurements
static const uint8_t SICK_CONF_SECTOR_RESERVED = 0x02; ///< Sector is reserved by Sick LD
static const uint8_t SICK_CONF_SECTOR_NORMAL_MEASUREMENT = 0x03; ///< Sector is returning measurements
static const uint8_t SICK_CONF_SECTOR_REFERENCE_MEASUREMENT = 0x04; ///< Sector can be used as reference measurement
/* Sick LD profile formats */
static const uint16_t SICK_SCAN_PROFILE_RANGE = 0x39FF; ///< Request sector scan data w/o any echo data
/*
* SICK_SCAN_PROFILE_RANGE format (0x39FF) interpretation:
* (See page 32 of telegram listing for fieldname definitions)
*
* Field Name | Send
* --------------------
* PROFILESENT | YES
* PROFILECOUNT | YES
* LAYERNUM | YES
* SECTORNUM | YES
* DIRSTEP | YES
* POINTNUM | YES
* TSTART | YES
* STARTDIR | YES
* DISTANCE-n | YES
* DIRECTION-n | NO
* ECHO-n | NO
* TEND | YES
* ENDDIR | YES
* SENSTAT | YES
*/
/* Sick LD profile formats */
static const uint16_t SICK_SCAN_PROFILE_RANGE_AND_ECHO = 0x3DFF; ///< Request sector scan data w/ echo data
/*
* SICK_SCAN_PROFILE_RANGE format (0x3DFF) interpretation:
* (See page 32 of telegram listing for fieldname definitions)
*
* Field Name | Send
* --------------------
* PROFILESENT | YES
* PROFILECOUNT | YES
* LAYERNUM | YES
* SECTORNUM | YES
* DIRSTEP | YES
* POINTNUM | YES
* TSTART | YES
* STARTDIR | YES
* DISTANCE-n | YES
* DIRECTION-n | NO
* ECHO-n | YES
* TEND | YES
* ENDDIR | YES
* SENSTAT | YES
*/
/* Masks for working with the Sick LD signals
*
* NOTE: Although the Sick LD manual defines the flag
* values for red and green LEDs the operation
* of these LEDs are reserved. So they can't
* be set by the device driver.
*/
static const uint8_t SICK_SIGNAL_LED_YELLOW_A = 0x01; ///< Mask for first yellow LED
static const uint8_t SICK_SIGNAL_LED_YELLOW_B = 0x02; ///< Mask for second yellow LED
static const uint8_t SICK_SIGNAL_LED_GREEN = 0x04; ///< Mask for green LED
static const uint8_t SICK_SIGNAL_LED_RED = 0x08; ///< Mask for red LED
static const uint8_t SICK_SIGNAL_SWITCH_0 = 0x10; ///< Mask for signal switch 0
static const uint8_t SICK_SIGNAL_SWITCH_1 = 0x20; ///< Mask for signal switch 1
static const uint8_t SICK_SIGNAL_SWITCH_2 = 0x40; ///< Mask for signal switch 2
static const uint8_t SICK_SIGNAL_SWITCH_3 = 0x80; ///< Mask for signal switch 3
/**
* \struct sick_ld_config_global_tag
* \brief A structure to aggregate the data used to configure the
* Sick LD global parameter values.
*/
/**
* \typedef sick_ld_config_global_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_config_global_tag {
uint16_t sick_sensor_id; ///< The single word sensor ID for the Sick unit
uint16_t sick_motor_speed; ///< Nominal motor speed value: 0x0005 to 0x0014 (5 to 20)
double sick_angle_step; ///< Difference between two laser pulse positions in 1/16th deg. (NOTE: this value must be a divisor of 5760 and be greater than 1)
} sick_ld_config_global_t;
/**
* \struct sick_ld_config_ethernet_tag
* \brief A structure to aggregate the data used to configure
* the Sick LD unit for Ethernet.
*
* \todo Eventually add similar config structures for the other protocols.
*/
/**
* \typedef sick_ld_config_ethernet_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_config_ethernet_tag {
uint16_t sick_ip_address[4]; ///< IP address in numerical form w/ leftmost part at sick_ip_address[0]
uint16_t sick_subnet_mask[4]; ///< Subnet mask for the network to which the Sick LD is assigned
uint16_t sick_gateway_ip_address[4]; ///< The address of the local gateway
uint16_t sick_node_id; ///< Single word address of the Sick LD
uint16_t sick_transparent_tcp_port; ///< The TCP/IP transparent port associated with the Sick LD
} sick_ld_config_ethernet_t;
/**
* \struct sick_ld_config_sector_tag
* \brief A structure to aggregate data used to define the
* Sick LD's sector configuration.
*/
/**
* \typedef sick_ld_config_sector_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_config_sector_tag {
uint8_t sick_num_active_sectors; ///< Number of active sectors (sectors that are actually being scanned)
uint8_t sick_num_initialized_sectors; ///< Number of sectors configured w/ a function other than "not initialized"
uint8_t sick_active_sector_ids[SICK_MAX_NUM_SECTORS]; ///< IDs of all active sectors
uint8_t sick_sector_functions[SICK_MAX_NUM_SECTORS]; ///< Function values associated w/ each of the Sick LD's sectors
double sick_sector_start_angles[SICK_MAX_NUM_SECTORS]; ///< Start angles for each initialized sector (deg)
double sick_sector_stop_angles[SICK_MAX_NUM_SECTORS]; ///< Stop angles for each sector (deg)
} sick_ld_config_sector_t;
/**
* \struct sick_ld_identity_tag
* \brief A structure to aggregate the fields that collectively
* define the identity of a Sick LD unit.
*/
/**
* \typedef sick_ld_identity_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_identity_tag {
std::string sick_part_number; ///< The Sick LD's part number
std::string sick_name; ///< The name assigned to the Sick
std::string sick_version; ///< The Sick LD's version number
std::string sick_serial_number; ///< The Sick LD's serial number
std::string sick_edm_serial_number; ///< The Sick LD's edm??? serial number
std::string sick_firmware_part_number; ///< The Sick LD's firmware part number
std::string sick_firmware_name; ///< The Sick LD's firmware name
std::string sick_firmware_version; ///< The Sick LD's firmware version
std::string sick_application_software_part_number; ///< The Sick LD's app. software part number
std::string sick_application_software_name; ///< The Sick LD's app. software name
std::string sick_application_software_version; ///< The Sick LD's app. software version
} sick_ld_identity_t;
/**
* \struct sick_ld_sector_data_tag
* \brief A structure to aggregate the fields that collectively
* define a sector in the scan area of the Sick LD unit.
*/
/**
* \typedef sick_ld_sector_data_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_sector_data_tag {
unsigned int sector_num; ///< The sector number in the scan area
unsigned int num_data_points; ///< The number of data points in the scan area
unsigned int timestamp_start; ///< The timestamp (in ms) corresponding to the time the first measurement in the sector was taken
unsigned int timestamp_stop; ///< The timestamp (in ms) corresponding to the time the last measurement in the sector was taken
unsigned int echo_values[SICK_MAX_NUM_MEASUREMENTS]; ///< The corresponding echo/reflectivity values
double angle_step; ///< The angle step used for the given sector (this should be the same for all sectors)
double angle_start; ///< The angle at which the first measurement in the sector was acquired
double angle_stop; ///< The angle at which the last measurement in the sector was acquired
double range_values[SICK_MAX_NUM_MEASUREMENTS]; ///< The corresponding range values (NOTE: The size of this array is intended to be large enough to accomodate various sector configs.)
double scan_angles[SICK_MAX_NUM_MEASUREMENTS]; ///< The scan angles corresponding to the respective measurements
} sick_ld_sector_data_t;
/**
* \struct sick_ld_scan_profile_tag
* \brief A structure to aggregate the fields that collectively
* define the profile of a single scan acquired from the
* Sick LD unit.
*/
/**
* \typedef sick_ld_scan_profile_t
* \brief Adopt c-style convention
*/
typedef struct sick_ld_scan_profile_tag {
unsigned int profile_number; ///< The number of profiles sent to the host (i.e. the current profile number)
unsigned int profile_counter; ///< The number of profiles gathered by the Sick LD
unsigned int layer_num; ///< The layer number associated with a scan (this will always be 0)
unsigned int sensor_status; ///< The status of the Sick LD sensor
unsigned int motor_status; ///< The status of the Sick LD motor
unsigned int num_sectors; ///< The number of sectors returned in the profile
sick_ld_sector_data_t sector_data[SICK_MAX_NUM_SECTORS]; ///< The sectors associated with the scan profile
} sick_ld_scan_profile_t;
/** Primary constructor */
SickLD( const std::string sick_ip_address = DEFAULT_SICK_IP_ADDRESS,
const uint16_t sick_tcp_port = DEFAULT_SICK_TCP_PORT );
/** Initializes the Sick LD unit (use scan areas defined in flash) */
void Initialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
/** Gets the sensor and motor mode of the unit */
void GetSickStatus( unsigned int &sick_sensor_mode, unsigned int &sick_motor_mode )
throw( SickIOException, SickTimeoutException );
/** Sets the temporal scan configuration (until power is cycled) */
void SetSickTempScanAreas( const double * active_sector_start_angles, const double * const active_sector_stop_angles,
const unsigned int num_active_sectors )
throw( SickTimeoutException, SickIOException, SickConfigException );
/** Sets the internal clock of the Sick LD unit */
void SetSickTimeAbsolute( const uint16_t absolute_clock_time, uint16_t &new_sick_clock_time )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Sets the internal clock of the Sick LD using the relative given time value */
void SetSickTimeRelative( const int16_t time_delta, uint16_t &new_sick_clock_time )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Gets the internal clock time of the Sick LD unit */
void GetSickTime( uint16_t &sick_time )
throw( SickIOException, SickTimeoutException, SickErrorException );
/** Sets the signal LEDs and switches */
void SetSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
throw( SickIOException, SickTimeoutException, SickErrorException );
/** Query the Sick for its current signal settings */
void GetSickSignals( uint8_t &sick_signal_flags ) throw( SickIOException, SickTimeoutException );
/** Enables nearfield suppressive filtering (in flash) */
void EnableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Disables nearfield suppressive filtering (in flash) */
void DisableNearfieldSuppression( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Acquires measurements and related data for all active sectors */
void GetSickMeasurements( double * const range_measurements,
unsigned int * const echo_measurements = NULL,
unsigned int * const num_measurements = NULL,
unsigned int * const sector_ids = NULL,
unsigned int * const sector_data_offsets = NULL,
double * const sector_step_angles = NULL,
double * const sector_start_angles = NULL,
double * const sector_stop_angles = NULL,
unsigned int * const sector_start_timestamps = NULL,
unsigned int * const sector_stop_timestamps = NULL )
throw( SickErrorException, SickIOException, SickTimeoutException, SickConfigException );
/** Attempts to set a new senor ID for the device (in flash) */
void SetSickSensorID( const unsigned int sick_sensor_id )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Attempts to set a new motor speed for the device (in flash) */
void SetSickMotorSpeed( const unsigned int sick_motor_speed )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Attempts to set a new scan resolution for the device (in flash) */
void SetSickScanResolution( const double sick_step_angle )
throw( SickTimeoutException, SickIOException, SickConfigException );
/** Attempts to set the global params and the active scan sectors for the device (in flash) */
void SetSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed,
const double sick_step_angle,
const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors )
throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
/** Attempts to set the active scan sectors for the device (in flash) */
void SetSickScanAreas( const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors )
throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
/** Resets the Sick LD using the given reset level */
void ResetSick( const unsigned int reset_level = SICK_WORK_SERV_RESET_INIT_CPU )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Returns the number of active/measuring sectors */
unsigned int GetSickNumActiveSectors( ) const;
/** Acquire the Sick LD's sensor ID */
unsigned int GetSickSensorID( ) const;
/** Acquire the Sick LD's current motor speed in Hz */
unsigned int GetSickMotorSpeed( ) const;
/** Acquire the Sick LD's current scan resolution */
double GetSickScanResolution( ) const;
/** Acquire the current IP address of the Sick */
std::string GetSickIPAddress( ) const;
/** Acquire the subnet mask for the Sick */
std::string GetSickSubnetMask( ) const;
/** Acquire the IP address of the Sick gateway */
std::string GetSickGatewayIPAddress( ) const;
/** Acquire the Sick LD's part number */
std::string GetSickPartNumber( ) const;
/** Acquire the Sick LD's name */
std::string GetSickName( ) const;
/** Acquire the Sick LD's version number */
std::string GetSickVersion( ) const;
/** Acquire the Sick LD's serial number */
std::string GetSickSerialNumber( ) const;
/** Acquire the Sick LD's EDM serial number */
std::string GetSickEDMSerialNumber( ) const;
/** Acquire the Sick LD's firmware part number */
std::string GetSickFirmwarePartNumber( ) const;
/** Acquire the Sick LD's firmware number */
std::string GetSickFirmwareName( ) const;
/** Acquire the Sick LD's firmware version */
std::string GetSickFirmwareVersion( ) const;
/** Acquire the Sick LD's application software part number */
std::string GetSickAppSoftwarePartNumber( ) const;
/** Acquire the Sick LD's application software name */
std::string GetSickAppSoftwareName( ) const;
/** Acquire the Sick LD's application software version number */
std::string GetSickAppSoftwareVersionNumber( ) const;
/** Acquire the Sick LD's status as a printable string */
std::string GetSickStatusAsString() const;
/** Acquire the Sick LD's identity as a printable string */
std::string GetSickIdentityAsString() const;
/** Acquire the Sick LD's global config as a printable string */
std::string GetSickGlobalConfigAsString() const;
/** Acquire the Sick LD's Ethernet config as a printable string */
std::string GetSickEthernetConfigAsString() const;
/** Acquire the Sick LD's sector config as a printable string */
std::string GetSickSectorConfigAsString() const;
/** Acquire the total scan area (in degrees) being scanned by the Sick LD */
double GetSickScanArea( ) const;
/** Prints the Sick LD's status information */
void PrintSickStatus( ) const;
/** Prints the Sick LD's identity information */
void PrintSickIdentity( ) const;
/** Prints the global configuration parameter values */
void PrintSickGlobalConfig( ) const;
/** Prints the Ethernet configuration parameter values */
void PrintSickEthernetConfig( ) const;
/** Prints the Sick Sector configuration */
void PrintSickSectorConfig( ) const;
/** Uninitializes the Sick LD unit */
void Uninitialize( ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
/** Destructor */
~SickLD();
private:
/** The Sick LD IP address */
std::string _sick_ip_address;
/** The Sick LD TCP port number */
uint16_t _sick_tcp_port;
/** Sick LD socket structure */
unsigned int _socket;
/** Sick LD socket address structure */
struct sockaddr_in _sick_inet_address_info;
/** The current sensor mode */
uint8_t _sick_sensor_mode;
/** The mode of the motor */
uint8_t _sick_motor_mode;
/** Indicates whether the Sick LD is currently streaming range data */
bool _sick_streaming_range_data;
/** Indicates whether the Sick LD is currently streaming range and echo data */
bool _sick_streaming_range_and_echo_data;
/** The identity structure for the Sick */
sick_ld_identity_t _sick_identity;
/** The current global configuration for the unit */
sick_ld_config_global_t _sick_global_config;
/** The current Ethernet configuration for the unit */
sick_ld_config_ethernet_t _sick_ethernet_config;
/** The current sector configuration for the unit */
sick_ld_config_sector_t _sick_sector_config;
/** Setup the connection parameters and establish TCP connection! */
void _setupConnection( ) throw( SickIOException, SickTimeoutException );
/** Synchronizes the driver state with the Sick LD (used for initialization) */
void _syncDriverWithSick( ) throw( SickIOException, SickTimeoutException, SickErrorException );
/** Set the function for a particular scan secto */
void _setSickSectorFunction( const uint8_t sector_number, const uint8_t sector_function,
const double sector_angle_stop, const bool write_to_flash = false )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Acquires the given Sector's function (i.e. current config) */
void _getSickSectorFunction( const uint8_t sector_num, uint8_t &sector_function, double &sector_stop_angle )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Sets the Sick LD to IDLE mode */
void _setSickSensorModeToIdle( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Sets the Sick LD to ROTATE mode */
void _setSickSensorModeToRotate( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Sets the Sick LD to MEASURE mode */
void _setSickSensorModeToMeasure( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Sets the Sick LD's sensor mode to IDLE (laser off, motor off) */
void _setSickSensorMode( const uint8_t new_sick_sensor_mode )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Requests n range measurement profiles from the Sick LD */
void _getSickScanProfiles( const uint16_t profile_format, const uint16_t num_profiles = DEFAULT_SICK_NUM_SCAN_PROFILES )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Parses a sequence of bytes and populates the profile_data struct w/ the results */
void _parseScanProfile( uint8_t * const src_buffer, sick_ld_scan_profile_t &profile_data ) const;
/** Cancels the active data stream */
void _cancelSickScanProfiles( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Turns nearfield suppression on/off */
void _setSickFilter( const uint8_t suppress_code )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Stores an image of the Sick LD's identity locally */
void _getSickIdentity( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for its sensor and motor status */
void _getSickStatus( ) throw( SickTimeoutException, SickIOException );
/** Sets the Sick LD's global configuration (in flash) */
void _setSickGlobalConfig( const uint8_t sick_sensor_id, const uint8_t sick_motor_speed, const double sick_angle_step )
throw( SickErrorException, SickTimeoutException, SickIOException );
/** Query the Sick for its global configuration parameters */
void _getSickGlobalConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Query the Sick for its Ethernet configuration parameters */
void _getSickEthernetConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Acquires the configuration (function and stop angle) for each sector */
void _getSickSectorConfig( ) throw( SickErrorException, SickTimeoutException, SickIOException );
/** Query the Sick for ID information */
void _getIdentificationString( const uint8_t id_request_code, std::string &id_return_string )
throw( SickTimeoutException, SickIOException );
/** Query the Sick for its sensor part number */
void _getSensorPartNumber( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for its assigned name */
void _getSensorName( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for its version number */
void _getSensorVersion( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for its serial number */
void _getSensorSerialNumber( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for its EDM unit's serial number */
void _getSensorEDMSerialNumber( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for the part number of its firmware */
void _getFirmwarePartNumber( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for the name of its firmware */
void _getFirmwareName( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for the version of the firmware */
void _getFirmwareVersion( ) throw( SickTimeoutException, SickIOException );
/** Query the part number of the application software */
void _getApplicationSoftwarePartNumber( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for the application name */
void _getApplicationSoftwareName( ) throw( SickTimeoutException, SickIOException );
/** Query the Sick for the application software version */
void _getApplicationSoftwareVersion( ) throw( SickTimeoutException, SickIOException );
/** Allows setting the global parameters and scan area definition (in flash) */
void _setSickGlobalParamsAndScanAreas( const unsigned int sick_motor_speed, const double sick_step_angle,
const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors )
throw( SickTimeoutException, SickIOException, SickConfigException, SickErrorException );
/** Allows setting a temporary (until a device reset) sector configuration on the device */
void _setSickTemporaryScanAreas( const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors )
throw( SickTimeoutException, SickIOException, SickConfigException );
/** Sets the sick sector configuration */
void _setSickSectorConfig( const unsigned int * const sector_functions, const double * const sector_stop_angles,
const unsigned int num_sectors, const bool write_to_flash = false )
throw( SickErrorException, SickTimeoutException, SickIOException, SickConfigException );
/** Sets the signals for the device */
void _setSickSignals( const uint8_t sick_signal_flags = DEFAULT_SICK_SIGNAL_SET )
throw( SickIOException, SickTimeoutException, SickErrorException );
/** Send a message, get the reply from the Sick LD and check it */
void _sendMessageAndGetReply( const SickLDMessage &send_message, SickLDMessage &recv_message,
const unsigned int timeout_value = DEFAULT_SICK_MESSAGE_TIMEOUT )
throw( SickIOException, SickTimeoutException );
/** Flushed the TCP receive buffer */
void _flushTCPRecvBuffer( ) throw ( SickIOException, SickThreadException );
/** Teardown the connection to the Sick LD */
void _teardownConnection( ) throw( SickIOException );
/** Generates a device-ready sector set given only an active sector spec. */
void _generateSickSectorConfig( const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors,
const double sick_step_angle,
unsigned int * const sector_functions,
double * const sector_stop_angles,
unsigned int &num_sectors ) const;
/** Converts odometry ticks to an equivalent angle */
double _ticksToAngle( const uint16_t ticks ) const;
/** Converts angle to an equivalent representation in odometer ticks */
uint16_t _angleToTicks( const double angle ) const;
/** Computes the mean pulse frequency for the given config */
double _computeMeanPulseFrequency( const double active_scan_area, const double curr_motor_speed,
const double curr_angular_resolution ) const;
/** Computes the total pulse frequency for the given config */
double _computeMaxPulseFrequency( const double total_scan_area, const double curr_motor_speed,
const double curr_angular_resolution ) const;
/** Indicates whether a given sensor ID is valid for the device */
bool _validSickSensorID( const unsigned int sick_sensor_id ) const;
/** Indicates whether a given motor speed is valid for the device */
bool _validSickMotorSpeed( const unsigned int sick_motor_speed ) const;
/** Indicates whether a given motor speed is valid for the device */
bool _validSickScanResolution( const double sick_step_angle, const double * const active_sector_start_angles,
const double * const active_sector_stop_angles, const unsigned int num_active_sectors ) const;
/** Indicates whether the given configuration yields a valid max and mean pulse frequency */
bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle ) const;
/** Indicates whether the given configuration yields a valid max and mean pulse frequency */
bool _validPulseFrequency( const unsigned int sick_motor_speed, const double sick_step_angle,
const double * const active_sector_start_angles,
const double * const active_sector_stop_angles,
const unsigned int num_active_sectors ) const;
/** Returns the scanning area for the device given the current sector configuration */
double _computeScanArea( const double sick_step_angle, const double * const sector_start_angles,
const double * const sector_stop_angles, const unsigned int num_sectors ) const;
/** Reorders given sector angle sets */
void _sortScanAreas( double * const sector_start_angles, double * const sector_stop_angles,
const unsigned int num_sectors ) const;
/** Checks the given sector arguments for overlapping regions yielding an invalid configuration */
bool _validActiveSectors( const double * const sector_start_angles, const double * const sector_stop_angles,
const unsigned int num_active_sectors ) const;
/** Indicates whether the supplied profile format is currently supported by the driver */
bool _supportedScanProfileFormat( const uint16_t profile_format ) const;
/** Prints data corresponding to a single scan sector (data obtained using GET_PROFILE) */
void _printSectorProfileData( const sick_ld_sector_data_t &sector_data ) const;
/** Prints the data corresponding to the given scan profile (for debugging purposes) */
void _printSickScanProfile( const sick_ld_scan_profile_t profile_data, const bool print_sector_data = true ) const;
/** Returns the corresponding work service subcode required to transition the Sick LD to the given sensor mode. */
uint8_t _sickSensorModeToWorkServiceSubcode( const uint8_t sick_sensor_mode ) const;
/** Converts _sick_sensor_mode to a representative string */
std::string _sickSensorModeToString( const uint8_t sick_sensor_mode ) const;
/** Converts _sick_motor_mode to a representative string */
std::string _sickMotorModeToString( const uint8_t sick_motor_mode ) const;
/** Converts the specified trans measurement mode return value to a string */
std::string _sickTransMeasureReturnToString( const uint8_t return_value ) const;
/** Converts the specified reset level to a representative string */
std::string _sickResetLevelToString( const uint16_t reset_level ) const;
/** Converts Sick LD sector configuration word to a representative string */
std::string _sickSectorFunctionToString( const uint16_t sick_sector_function ) const;
/** Converts a given scan profile format to a string for friendlier output */
std::string _sickProfileFormatToString( const uint16_t profile_format ) const;
/** Prints the initialization footer */
void _printInitFooter( ) const;
};
} //namespace SickToolbox
#endif /* SICK_LD_HH */

50
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLDBufferMonitor.hh Normal file
View File

@ -0,0 +1,50 @@
/*!
* \file SickLDBufferMonitor.hh
* \brief Defines a class for monitoring the receive
* buffer when interfacing w/ a Sick LMS LIDAR.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LD_BUFFER_MONITOR_HH
#define SICK_LD_BUFFER_MONITOR_HH
#define DEFAULT_SICK_BYTE_TIMEOUT (35000) ///< Max allowable time between consecutive bytes
/* Definition dependencies */
#include "SickLDMessage.hh"
#include "SickBufferMonitor.hh"
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/*!
* \brief A class for monitoring the receive buffer when interfacing with a Sick LD LIDAR
*/
class SickLDBufferMonitor : public SickBufferMonitor< SickLDBufferMonitor, SickLDMessage > {
public:
/** A standard constructor */
SickLDBufferMonitor( );
/** A method for extracting a single message from the stream */
void GetNextMessageFromDataStream( SickLDMessage &sick_message ) throw( SickIOException );
/** A standard destructor */
~SickLDBufferMonitor( );
};
} /* namespace SickToolbox */
#endif /* SICK_LD_BUFFER_MONITOR_HH */

80
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLDMessage.hh Normal file
View File

@ -0,0 +1,80 @@
/*!
* \file SickLDMessage.hh
* \brief Defines the class SickLDMessage.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LD_MESSAGE_HH
#define SICK_LD_MESSAGE_HH
/* Definition dependencies */
#include <string.h>
#include <arpa/inet.h>
#include "SickMessage.hh"
#define SICK_LD_MSG_HEADER_LEN (8) ///< Sick LD message header length in bytes
#define SICK_LD_MSG_PAYLOAD_MAX_LEN (5816) ///< Sick LD maximum payload length
#define SICK_LD_MSG_TRAILER_LEN (1) ///< Sick LD length of the message trailer
/* Associate the namespace */
namespace SickToolbox {
/**
* \class SickLDMessage
* \brief A class to represent all messages sent to and from the Sick LD unit.
*/
class SickLDMessage : public SickMessage< SICK_LD_MSG_HEADER_LEN, SICK_LD_MSG_PAYLOAD_MAX_LEN, SICK_LD_MSG_TRAILER_LEN > {
public:
/** A standard constructor */
SickLDMessage( );
/** Constructs a packet by using BuildMessage */
SickLDMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Constructs a packet using ParseMessage() */
SickLDMessage( const uint8_t * const message_buffer );
/** Construct a well-formed raw packet */
void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Populates fields from a (well-formed) raw packet */
void ParseMessage( const uint8_t * const message_buffer );
/** Get the length of the service code associated with the message */
uint8_t GetServiceCode( ) const { return _message_buffer[8]; }
/** Get the service sub-code associated with the message */
uint8_t GetServiceSubcode( ) const { return _message_buffer[9]; }
/** Get the checksum for the packet */
uint8_t GetChecksum( ) const { return _message_buffer[_message_length-1]; }
/** A debugging function that prints the contents of the frame. */
void Print( ) const;
/** Destructor */
~SickLDMessage( );
private:
/** Computes the checksum of the frame.
* NOTE: Uses XOR of single bytes over packet payload data.
*/
uint8_t _computeXOR( const uint8_t * const data, const uint32_t length );
};
} /* namespace SickToolbox */
#endif /* SICK_LD_MESSAGE_HH */

120
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLDUtility.hh Normal file
View File

@ -0,0 +1,120 @@
/*!
* \file SickLDUtility.hh
* \brief Defines simple utility functions for working with the Sick LD.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include "SickConfig.hh"
/**
* \def REVERSE_BYTE_ORDER_16
* \brief Reverses the byte order of the given 16 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) )
/**
* \def REVERSE_BYTE_ORDER_32
* \brief Reverses the byte order of the given 32 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) )
/* Associate the namespace */
namespace SickToolbox {
#ifndef WORDS_BIGENDIAN
/* NOTE: The following functions are necessary since the Sick LD doesn't adopt the
* convention of converting from network byte order.
*/
/**
* \brief Converts host byte order (little-endian) to Sick LD byte order (big-endian)
* \param value The 2-byte value to convert to big-endian
* \return Value in Sick LD byte order (big-endian)
*/
inline uint16_t host_to_sick_ld_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts host byte order (little-endian) to Sick LD byte order (big-endian)
* \param value The 4-byte value to convert to big-endian
* \return Value in Sick LD byte order (big-endian)
*/
inline uint32_t host_to_sick_ld_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
/**
* \brief Converts Sick LD byte order (big-endian) to host byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint16_t sick_ld_to_host_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts Sick LD byte order (big-endian) to host byte order (little-endian)
* \param value The 4-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint32_t sick_ld_to_host_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
#else // The host has a big-endian architecture
/**
* \brief Converts host byte order (big-endian) to Sick LD byte order (big-endian)
* \param value The 2-byte value to convert to big-endian
* \return Value in Sick LD byte order (big-endian)
*/
inline uint16_t host_to_sick_ld_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts host byte order (big-endian) to Sick LD byte order (big-endian)
* \param value The 4-byte value to convert to big-endian
* \return Value in Sick LD byte order (big-endian)
*/
inline uint32_t host_to_sick_ld_byte_order( uint32_t value ) {
return value;
}
/**
* \brief Converts Sick LD byte order (big-endian) to host byte order (big-endian)
* \param value The 2-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint16_t sick_ld_to_host_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts Sick LD byte order (big-endian) to host byte order (big-endian)
* \param value The 4-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint32_t sick_ld_to_host_byte_order( uint32_t value ) {
return value;
}
#endif /* _LITTLE_ENDIAN_HOST */
/*
* NOTE: Other utility functions can be defined here
*/
} //namespace SickToolbox

452
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLIDAR.hh Normal file
View File

@ -0,0 +1,452 @@
/*!
* \file SickLIDAR.hh
* \brief Defines the abstract parent class for defining
* a Sick LIDAR device driver.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/**
* \mainpage The Sick LIDAR Matlab/C++ Toolbox
* \author Jason C. Derenick and Thomas H. Miller.
* \version 1.0
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LIDAR
#define SICK_LIDAR
/* Definition dependencies */
#include <new>
#include <string>
#include <iomanip>
#include <iostream>
#include <fcntl.h>
#include <pthread.h>
#include <arpa/inet.h>
#include <sys/time.h>
#include <unistd.h>
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/**
* \class SickLIDAR
* \brief Provides an abstract parent for all Sick LIDAR devices
*/
template < class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
class SickLIDAR {
public:
/** The primary constructor */
SickLIDAR( );
/** Indicates whether device is initialized */
bool IsInitialized() { return _sick_initialized; }
/** A virtual destructor */
virtual ~SickLIDAR( );
protected:
/** Sick device file descriptor */
int _sick_fd;
/** A flag to indicated whether the device is properly initialized */
bool _sick_initialized;
/** A pointer to the driver's buffer monitor */
SICK_MONITOR_CLASS *_sick_buffer_monitor;
/** Indicates whether the Sick buffer monitor is running */
bool _sick_monitor_running;
/** A method for setting up a general connection */
virtual void _setupConnection( ) = 0;
/** A method for tearing down a connection to the Sick */
virtual void _teardownConnection( ) = 0;
/** Starts the driver listening for messages */
void _startListening( ) throw( SickThreadException );
/** Stops the driver from listening */
void _stopListening( ) throw( SickThreadException );
/** Indicates whether there is a monitor currently running */
bool _monitorRunning( ) const { return _sick_monitor_running; }
/** Make the associated file descriptor non blocking */
void _setBlockingIO( ) const throw ( SickIOException );
/** Make the associated file descriptor non blocking */
void _setNonBlockingIO( ) const throw ( SickIOException );
/** Send a message to the Sick LD (allows specifying min time between transmitted bytes) */
void _sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
throw( SickIOException );
/** Acquire the next message from the message container */
void _recvMessage( SICK_MSG_CLASS &sick_message, const unsigned int timeout_value ) const throw ( SickTimeoutException );
/** Search the stream for a payload with a particular "header" byte string */
void _recvMessage( SICK_MSG_CLASS &sick_message,
const uint8_t * const byte_sequence,
const unsigned int byte_sequence_length,
const unsigned int timeout_value ) const throw ( SickTimeoutException );
/** An inline function for computing elapsed time */
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); }
/** Sends a request to the Sick and acquires looks for the reply */
virtual void _sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
SICK_MSG_CLASS &recv_message,
const uint8_t * const byte_sequence,
const unsigned int byte_sequence_length,
const unsigned int byte_interval,
const unsigned int timeout_value,
const unsigned int num_tries ) throw( SickTimeoutException, SickIOException);
};
/**
* \brief Initializes the buffer monitor
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::SickLIDAR( ) :
_sick_fd(0), _sick_initialized(false), _sick_buffer_monitor(NULL), _sick_monitor_running(false) {
try {
/* Attempt to instantiate a new SickBufferMonitor for the device */
_sick_buffer_monitor = new SICK_MONITOR_CLASS;
}
catch ( std::bad_alloc &allocation_exception ) {
std::cerr << "SickLIDAR::SickLIDAR: Allocation error - " << allocation_exception.what() << std::endl;
}
}
/**
* \brief Destructor tears down buffer monitor
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::~SickLIDAR( ) {
/* Deallocate the buffer monitor */
if (_sick_buffer_monitor) {
delete _sick_buffer_monitor;
}
}
/**
* \brief Activates the buffer monitor for the driver
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_startListening( ) throw( SickThreadException ) {
/* Try to start the monitor */
try {
_sick_buffer_monitor->StartMonitor(_sick_fd);
}
/* Handle a thread exception */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
throw;
}
/* Handle a thread exception */
catch(...) {
std::cerr << "SickLIDAR::_startListening: Unknown exception!!!" << std::endl;
throw;
}
/* Set the flag */
_sick_monitor_running = true;
}
/**
* \brief Activates the buffer monitor for the driver
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_stopListening( ) throw( SickThreadException ) {
/* Try to start the monitor */
try {
_sick_buffer_monitor->StopMonitor();
}
/* Handle a thread exception */
catch(SickThreadException &sick_thread_exception) {
std::cerr << sick_thread_exception.what() << std::endl;
throw;
}
/* Handle a thread exception */
catch(...) {
std::cerr << "SickLIDAR::_stopListening: Unknown exception!!!" << std::endl;
throw;
}
/* Reset the flag */
_sick_monitor_running = false;
}
/**
* \brief A simple method for setting blocking I/O
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setBlockingIO( ) const throw( SickIOException ) {
/* Read the flags */
int fd_flags = 0;
if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
}
/* Set the new flags */
if(fcntl(_sick_fd,F_SETFL,fd_flags & (~O_NONBLOCK)) < 0) {
throw SickIOException("SickLIDAR::_setNonBlocking: fcntl failed!");
}
}
/**
* \brief A simple method for setting non-blocking I/O
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_setNonBlockingIO( ) const throw ( SickIOException ) {
/* Read the flags */
int fd_flags = 0;
if((fd_flags = fcntl(_sick_fd,F_GETFL)) < 0) {
throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
}
/* Set the new flags */
if(fcntl(_sick_fd,F_SETFL,fd_flags | O_NONBLOCK) < 0) {
throw SickIOException("SickLIDAR::_setNonBlockingIO: fcntl failed!");
}
}
/**
* \brief Sends a message to the Sick device
* \param &sick_message A reference to the well-formed message that is to be sent to the Sick
* \param byte_interval Minimum time in microseconds between transmitted bytes
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessage( const SICK_MSG_CLASS &sick_message, const unsigned int byte_interval ) const
throw( SickIOException ) {
uint8_t message_buffer[SICK_MSG_CLASS::MESSAGE_MAX_LENGTH] = {0};
/* Copy the given message and get the message length */
sick_message.GetMessage(message_buffer);
unsigned int message_length = sick_message.GetMessageLength();
/* Check whether a transmission delay between bytes is requested */
if (byte_interval == 0) {
/* Write the message to the stream */
if ((unsigned int)write(_sick_fd,message_buffer,message_length) != message_length) {
throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
}
}
else {
/* Write the message to the unit one byte at a time */
for (unsigned int i = 0; i < message_length; i++) {
/* Write a single byte to the stream */
if (write(_sick_fd,&message_buffer[i],1) != 1) {
throw SickIOException("SickLIDAR::_sendMessage: write() failed!");
}
/* Some time between bytes (Sick LMS 2xx likes this) */
usleep(byte_interval);
}
}
}
/**
* \brief Attempt to acquire the latest available message from the device
* \param &sick_message A reference to the container that will hold the most recent message
* \param timeout_value The time in secs to wait before throwing a timeout error
* \return True if a new message was received, False otherwise
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
const unsigned int timeout_value ) const throw ( SickTimeoutException ) {
/* Timeval structs for handling timeouts */
struct timeval beg_time, end_time;
/* Acquire the elapsed time since epoch */
gettimeofday(&beg_time,NULL);
/* Check the shared object */
while(!_sick_buffer_monitor->GetNextMessageFromMonitor(sick_message)) {
/* Sleep a little bit */
usleep(1000);
/* Check whether the allowed time has expired */
gettimeofday(&end_time,NULL);
if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
throw SickTimeoutException("SickLIDAR::_recvMessage: Timeout occurred!");
}
}
}
/**
* \brief Attempt to acquire a message having a payload beginning w/ the given byte sequence
* \param &sick_message A reference to the container that will hold the most recent message
* \param *byte_sequence The byte sequence that is expected to lead off the payload in the packet (e.g. service codes, etc...)
* \param byte_sequence_length The number of bytes in the given byte_sequence
* \param timeout_value The time in usecs to wait before throwing a timeout error
* \return True if a new message was received, False otherwise
*
* NOTE: This method is intended to be a helper for _sendMessageAndGetReply
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_recvMessage( SICK_MSG_CLASS &sick_message,
const uint8_t * const byte_sequence,
const unsigned int byte_sequence_length,
const unsigned int timeout_value ) const throw( SickTimeoutException ) {
/* Define a buffer */
uint8_t payload_buffer[SICK_MSG_CLASS::MESSAGE_PAYLOAD_MAX_LENGTH];
/* Timeval structs for handling timeouts */
struct timeval beg_time, end_time;
/* A container for the message */
SICK_MSG_CLASS curr_message;
/* Get the elapsed time since epoch */
gettimeofday(&beg_time,NULL);
/* Check until it is found or a timeout */
for(;;) {
/* Attempt to acquire the message */
unsigned int i = 0;
if (_sick_buffer_monitor->GetNextMessageFromMonitor(curr_message)) {
/* Extract the payload subregion */
curr_message.GetPayloadSubregion(payload_buffer,0,byte_sequence_length-1);
/* Match the byte sequence */
for (i=0; (i < byte_sequence_length) && (payload_buffer[i] == byte_sequence[i]); i++);
/* Our message was found! */
if (i == byte_sequence_length) {
sick_message = curr_message;
break;
}
}
/* Sleep a little bit */
usleep(1000);
/* Check whether the allowed time has expired */
gettimeofday(&end_time,NULL);
if (_computeElapsedTime(beg_time,end_time) > timeout_value) {
throw SickTimeoutException();
}
}
}
/**
* \param sick_send_frame A sick frame to be sent to the LMS
* \param sick_receive_frame A sick frame to hold the response (expected or unexpected) of the LMS
* \param num_tries The number of times to send the frame in the event the LMS fails to reply
* \param timeout The epoch to wait before considering a sent frame lost
* \return True if the message was sent and the expected reply was received
*/
template< class SICK_MONITOR_CLASS, class SICK_MSG_CLASS >
void SickLIDAR< SICK_MONITOR_CLASS, SICK_MSG_CLASS >::_sendMessageAndGetReply( const SICK_MSG_CLASS &send_message,
SICK_MSG_CLASS &recv_message,
const uint8_t * const byte_sequence,
const unsigned int byte_sequence_length,
const unsigned int byte_interval,
const unsigned int timeout_value,
const unsigned int num_tries )
throw( SickTimeoutException, SickIOException ) {
/* Send the message for at most num_tries number of times */
for(unsigned int i = 0; i < num_tries; i++) {
try {
/* Send the frame to the unit */
_sendMessage(send_message,byte_interval);
/* Wait for the reply! */
_recvMessage(recv_message,byte_sequence,byte_sequence_length,timeout_value);
/* message was found! */
break;
}
/* Handle a timeout! */
catch (SickTimeoutException &sick_timeout) {
/* Check if it was found! */
if (i == num_tries - 1) {
throw SickTimeoutException("SickLIDAR::_sendMessageAndGetReply: Attempted max number of tries w/o success!");
}
/* Display the number of tries remaining! */
std::cerr << sick_timeout.what() << " " << num_tries - i - 1 << " tries remaining" << std::endl;
}
/* Handle write buffer exceptions */
catch (SickIOException &sick_io_error) {
std::cerr << sick_io_error.what() << std::endl;
throw;
}
/* A safety net */
catch (...) {
std::cerr << "SickLIDAR::_sendMessageAndGetReply: Unknown exception!!!" << std::endl;
throw;
}
}
}
} /* namespace SickToolbox */
#endif /* SICK_LIDAR */

335
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS1xx.hh Normal file
View File

@ -0,0 +1,335 @@
/*!
* \file SickLMS1xx.hh
* \brief Defines the SickLMS1xx class for working with the
* Sick LMS1xx laser range finders.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_1XX_HH
#define SICK_LMS_1XX_HH
/* Macros */
#define DEFAULT_SICK_LMS_1XX_IP_ADDRESS "192.168.0.1" ///< Default IP Address
#define DEFAULT_SICK_LMS_1XX_TCP_PORT (2111) ///< Sick LMS 1xx TCP/IP Port
#define DEFAULT_SICK_LMS_1XX_CONNECT_TIMEOUT (1000000) ///< Max time for establishing connection (usecs)
#define DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT (5000000) ///< Max time for reply (usecs)
#define DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT (60000000) ///< Max time it should take to change status
#define SICK_LMS_1XX_SCAN_AREA_MIN_ANGLE (-450000) ///< -45 degrees (1/10000) degree
#define SICK_LMS_1XX_SCAN_AREA_MAX_ANGLE (2250000) ///< 225 degrees (1/10000) degree
/* Definition dependencies */
#include <string>
#include <arpa/inet.h>
#include "SickLIDAR.hh"
#include "SickLMS1xxBufferMonitor.hh"
#include "SickLMS1xxMessage.hh"
#include "SickException.hh"
/**
* \namespace SickToolbox
* \brief Encapsulates the Sick LIDAR Matlab/C++ toolbox
*/
namespace SickToolbox {
/**
* \class SickLMS1xx
* \brief Provides a simple driver interface for working with the
* Sick LD-OEM/Sick LD-LRS long-range models via Ethernet.
*/
class SickLMS1xx : public SickLIDAR< SickLMS1xxBufferMonitor, SickLMS1xxMessage > {
public:
static const int SICK_LMS_1XX_MAX_NUM_MEASUREMENTS = 1082; ///< LMS 1xx max number of measurements
/*!
* \enum sick_lms_1xx_status_t
* \brief Defines the Sick LMS 1xx status.
* This enum lists all of the Sick LMS 1xx status.
*/
enum sick_lms_1xx_status_t {
SICK_LMS_1XX_STATUS_UNKNOWN = 0x00, ///< LMS 1xx status undefined
SICK_LMS_1XX_STATUS_INITIALIZATION = 0x01, ///< LMS 1xx initializing
SICK_LMS_1XX_STATUS_CONFIGURATION = 0x02, ///< LMS 1xx configuration
SICK_LMS_1XX_STATUS_IDLE = 0x03, ///< LMS 1xx is idle
SICK_LMS_1XX_STATUS_ROTATED = 0x04, ///< LMS 1xx mirror rotating
SICK_LMS_1XX_STATUS_IN_PREP = 0x05, ///< LMS 1xx in preparation
SICK_LMS_1XX_STATUS_READY = 0x06, ///< LMS 1xx is ready
SICK_LMS_1XX_STATUS_READY_FOR_MEASUREMENT = 0x07 ///< LMS 1xx is ready to give measurements
};
/*!
* \enum sick_lms_1xx_scan_format_t
* \brief Defines the Sick LMS 1xx scan types
* This enum is for specifiying the desired scan returns.
*/
enum sick_lms_1xx_scan_format_t {
SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_NONE = 0x00, ///< Single-pulse dist, no reflect
SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_8BIT = 0x01, ///< Single-pulse dist, 8bit reflect
SICK_LMS_1XX_SCAN_FORMAT_DIST_SINGLE_PULSE_REFLECT_16BIT = 0x02, ///< Single-pulse dist, 16bit reflect
SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_NONE = 0x03, ///< Double-pulse dist, no reflect
SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_8BIT = 0x04, ///< Double-pulse dist, 8bit reflect
SICK_LMS_1XX_SCAN_FORMAT_DIST_DOUBLE_PULSE_REFLECT_16BIT = 0x05, ///< Double-pulse dist, 16bit reflect
SICK_LMS_1XX_SCAN_FORMAT_UNKNOWN = 0xFF ///< Unknown format
};
/*!
* \enum sick_lms_1xx_scan_freq_t
* \brief Defines the Sick LMS 1xx scan frequency.
* This enum lists all of the Sick LMS 1xx scan frequencies.
*/
enum sick_lms_1xx_scan_freq_t {
SICK_LMS_1XX_SCAN_FREQ_UNKNOWN = 0x00, ///< LMS 1xx scan freq unknown
SICK_LMS_1XX_SCAN_FREQ_25 = 0x09C4, ///< LMS 1xx scan freq 25Hz
SICK_LMS_1XX_SCAN_FREQ_50 = 0X1388 ///< LMS 1xx scan freq 50Hz
};
/*!
* \enum sick_lms_1xx_scan_res_t
* \brief Defines the Sick LMS 1xx scan resolution.
* This enum lists all of the Sick LMS 1xx scan resolutions.
*/
enum sick_lms_1xx_scan_res_t {
SICK_LMS_1XX_SCAN_RES_UNKNOWN = 0x00, ///< LMS 1xx scab res unknown
SICK_LMS_1XX_SCAN_RES_25 = 0x09C4, ///< LMS 1xx scan res 0.25 deg
SICK_LMS_1XX_SCAN_RES_50 = 0x1388 ///< LMS 1xx scan res 0.50 deg
};
/** Primary constructor */
SickLMS1xx( const std::string sick_ip_address = DEFAULT_SICK_LMS_1XX_IP_ADDRESS,
const uint16_t sick_tcp_port = DEFAULT_SICK_LMS_1XX_TCP_PORT );
/** Initializes the Sick LD unit (use scan areas defined in flash) */
void Initialize( const bool disp_banner = true ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
/** Sets the Sick LMS 1xx scan frequency and scan resolution */
void SetSickScanFreqAndRes( const sick_lms_1xx_scan_freq_t scan_freq,
const sick_lms_1xx_scan_res_t scan_res ) throw( SickTimeoutException, SickIOException, SickErrorException );
/** Get the Sick LMS 1xx scan frequency */
sick_lms_1xx_scan_freq_t GetSickScanFreq( ) const throw ( SickIOException );
/** Get the Sick LMS 1xx scan resolution */
sick_lms_1xx_scan_res_t GetSickScanRes( ) const throw ( SickIOException );
/** Get the Sick LMS 1xx scan start angle */
double GetSickStartAngle( ) const throw ( SickIOException );
/** Get the Sick LMS 1xx scan stop angle */
double GetSickStopAngle( ) const throw ( SickIOException );
/** Sets the sick scan data format */
void SetSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
/** Get the Sick Range Measurements */
void GetSickMeasurements( unsigned int * const range_1_vals,
unsigned int * const range_2_vals,
unsigned int * const reflect_1_vals,
unsigned int * const reflect_2_vals,
unsigned int & num_measurements,
unsigned int * const dev_status = NULL ) throw ( SickIOException, SickConfigException, SickTimeoutException );
/** Uninitializes the Sick LD unit */
void Uninitialize( const bool disp_banner = true ) throw( SickIOException, SickTimeoutException, SickErrorException, SickThreadException );
/** Utility function for converting integer to scan frequency */
sick_lms_1xx_scan_freq_t IntToSickScanFreq( const int scan_freq ) const;
/** Utility function for converting scan frequency to integer */
int SickScanFreqToInt( const sick_lms_1xx_scan_freq_t scan_freq ) const;
/** Utility function for converting double to scan resolution */
sick_lms_1xx_scan_res_t DoubleToSickScanRes( const double scan_res ) const;
/** Utility function for converting scan resolution to double */
double SickScanResToDouble( const sick_lms_1xx_scan_res_t scan_res ) const;
/** Destructor */
~SickLMS1xx();
private:
/*!
* \struct sick_lms_1xx_scan_config_tag
* \brief A structure for aggregrating the
* Sick LMS 1xx configuration params.
*/
/*!
* \typedef sick_lms_1xx_scan_config_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_1xx_scan_config_tag {
sick_lms_1xx_scan_freq_t sick_scan_freq; ///< Sick scan frequency
sick_lms_1xx_scan_res_t sick_scan_res; ///< Sick scan resolution
int32_t sick_start_angle; ///< Sick scan area start angle
int32_t sick_stop_angle; ///< Sick scan area stop angle
} sick_lms_1xx_scan_config_t;
/** The Sick LMS 1xx IP address */
std::string _sick_ip_address;
/** The Sick LMS 1xx TCP port number */
uint16_t _sick_tcp_port;
/** Sick LMS 1xx socket address struct */
struct sockaddr_in _sick_inet_address_info;
/** Sick LMS 1xx configuration struct */
sick_lms_1xx_scan_config_t _sick_scan_config;
/** Sick LMS 1xx current scan data format */
sick_lms_1xx_scan_format_t _sick_scan_format;
/** Sick LMS 1xx configuration struct */
sick_lms_1xx_status_t _sick_device_status;
/** Sick LMS 1xx temperature status */
bool _sick_temp_safe;
/** Sick LMS 1xx streaming status */
bool _sick_streaming;
/** Setup the connection parameters and establish TCP connection! */
void _setupConnection( ) throw( SickIOException, SickTimeoutException );
/** Re-initialized the device */
void _reinitialize( ) throw( SickIOException, SickThreadException, SickTimeoutException, SickErrorException );
/** Teardown the connection to the Sick LD */
void _teardownConnection( ) throw( SickIOException );
/** Acquire the latest Sick LMS's status */
void _updateSickStatus( ) throw( SickTimeoutException, SickIOException );
/** Acquire the Sick LMS's scan config */
void _getSickScanConfig( ) throw( SickTimeoutException, SickIOException );
/** Sets the scan configuration (volatile, does not write to EEPROM) */
void _setSickScanConfig( const sick_lms_1xx_scan_freq_t scan_freq,
const sick_lms_1xx_scan_res_t scan_res,
const int start_angle, const int stop_angle ) throw( SickTimeoutException, SickIOException, SickErrorException );
/** Set access mode for configuring device */
void _setAuthorizedClientAccessMode( ) throw( SickTimeoutException, SickErrorException, SickIOException );
/** Save configuration parameters to EEPROM */
void _writeToEEPROM( ) throw( SickTimeoutException, SickIOException );
/** Send the message w/o waiting for a reply */
void _sendMessage( const SickLMS1xxMessage &send_message ) const throw ( SickIOException );
/** Send the message and grab expected reply */
void _sendMessageAndGetReply( const SickLMS1xxMessage &send_message,
SickLMS1xxMessage &recv_message,
const std::string reply_command_code,
const std::string reply_command,
const unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_MESSAGE_TIMEOUT,
const unsigned int num_tries = 1 ) throw( SickIOException, SickTimeoutException );
/** Receive a message */
void _recvMessage( SickLMS1xxMessage &sick_message ) const throw ( SickTimeoutException );
/** Start device measuring */
void _startMeasuring( ) throw ( SickTimeoutException, SickIOException );
/** Stop device measuring */
void _stopMeasuring( ) throw ( SickTimeoutException, SickIOException );
/** Request a data data stream type */
void _requestDataStream( ) throw ( SickTimeoutException, SickConfigException, SickIOException );
/** Start streaming measurements */
void _startStreamingMeasurements( )throw( SickTimeoutException, SickIOException );
/** Stop streaming measurements */
void _stopStreamingMeasurements( const bool disp_banner = true ) throw( SickTimeoutException, SickIOException );
/** Set device to measuring mode */
void _checkForMeasuringStatus( unsigned int timeout_value = DEFAULT_SICK_LMS_1XX_STATUS_TIMEOUT ) throw( SickTimeoutException, SickIOException );
/** Sets the sick scan data format */
void _setSickScanDataFormat( const sick_lms_1xx_scan_format_t scan_format ) throw( SickTimeoutException, SickIOException, SickThreadException, SickErrorException );
/** Restore device to measuring mode */
void _restoreMeasuringMode( ) throw( SickTimeoutException, SickIOException );
/** Ensures a feasible scan area */
bool _validScanArea( const int start_angle, const int stop_angle ) const;
/** Utility function to convert int to status */
sick_lms_1xx_status_t _intToSickStatus( const int status ) const;
/** Utility function for printing Sick scan config */
void _printSickScanConfig( ) const;
/** Utility function for printing footer after initialization */
void _printInitFooter( ) const;
/** Utility function for returning scan format as string */
std::string _sickScanDataFormatToString( const sick_lms_1xx_scan_format_t scan_format ) const;
/** Utility function for converting sick freq to doubles */
double _convertSickAngleUnitsToDegs( const int sick_angle ) const { return ((double)sick_angle)/10000; }
/** Utility function for converting sick Hz values ints */
unsigned int _convertSickFreqUnitsToHz( const unsigned int sick_freq ) const { return (unsigned int)(((double)sick_freq)/100); }
/** Utility function to convert config error int to str */
std::string _intToSickConfigErrorStr( const int error ) const;
/** Utility function to locate substring in string */
bool _findSubString( const char * const str, const char * const substr, const unsigned int str_length, const unsigned int substr_length,
unsigned int &substr_pos, unsigned int start_pos = 0 ) const;
/** Utility function for extracting next integer from tokenized string */
char * _convertNextTokenToUInt( char * const str_buffer, unsigned int & num_val, const char * const delimeter = " " ) const;
};
/*!
* \typedef sick_lms_1xx_status_t
* \brief Makes working w/ SickLMS1xx::sick_lms_1xx_status_t a bit easier
*/
typedef SickLMS1xx::sick_lms_1xx_status_t sick_lms_1xx_status_t;
/*!
* \typedef sick_lms_1xx_scan_format_t
* \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_format_t a bit easier
*/
typedef SickLMS1xx::sick_lms_1xx_scan_format_t sick_lms_1xx_scan_format_t;
/*!
* \typedef sick_lms_1xx_scan_freq_t
* \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_freq_t a bit easier
*/
typedef SickLMS1xx::sick_lms_1xx_scan_freq_t sick_lms_1xx_scan_freq_t;
/*!
* \typedef sick_lms_1xx_scan_res_t
* \brief Makes working w/ SickLMS1xx::sick_lms_1xx_scan_res_t a bit easier
*/
typedef SickLMS1xx::sick_lms_1xx_scan_res_t sick_lms_1xx_scan_res_t;
} //namespace SickToolbox
#endif /* SICK_LMS_1XX_HH */

56
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS1xxBufferMonitor.hh Normal file
View File

@ -0,0 +1,56 @@
/*!
* \file SickLMS1xxBufferMonitor.hh
* \brief Defines a class for monitoring the receive
* buffer when interfacing w/ a Sick LMS 1xx
* laser range finder unit.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_1XX_BUFFER_MONITOR_HH
#define SICK_LMS_1XX_BUFFER_MONITOR_HH
#define DEFAULT_SICK_LMS_1XX_BYTE_TIMEOUT (100000) ///< Max allowable time between consecutive bytes
/* Definition dependencies */
#include "SickLMS1xxMessage.hh"
#include "SickBufferMonitor.hh"
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/*!
* \brief A class for monitoring the receive buffer when interfacing with a Sick LD LIDAR
*/
class SickLMS1xxBufferMonitor : public SickBufferMonitor< SickLMS1xxBufferMonitor, SickLMS1xxMessage > {
public:
/** A standard constructor */
SickLMS1xxBufferMonitor( );
/** A method for extracting a single message from the stream */
void GetNextMessageFromDataStream( SickLMS1xxMessage &sick_message ) throw( SickIOException );
/** A standard destructor */
~SickLMS1xxBufferMonitor( );
private:
/* A utility function for flushing the receive buffer */
void _flushTCPRecvBuffer( ) const throw ( SickIOException );
};
} /* namespace SickToolbox */
#endif /* SICK_LMS_1XX_BUFFER_MONITOR_HH */

83
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS1xxMessage.hh Normal file
View File

@ -0,0 +1,83 @@
/*!
* \file SickLMS1xx Message.hh
* \brief Defines the class SickLMS1xxMessage.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christoper R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_1XX_MESSAGE_HH
#define SICK_LMS_1XX_MESSAGE_HH
/* Macros */
#define SICK_LMS_1XX_MSG_HEADER_LEN (1) ///< Sick LMS 1xx message header length in bytes
#define SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN (30000) ///< Sick LMS 1xx maximum payload length
#define SICK_LMS_1XX_MSG_TRAILER_LEN (1) ///< Sick LMS 1xx length of the message trailer
/* Definition dependencies */
#include <string.h>
#include <arpa/inet.h>
#include "SickMessage.hh"
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/**
* \class SickLMS1xxMessage
* \brief A class to represent all messages sent to and from the Sick LMS 1xx unit.
*/
class SickLMS1xxMessage : public SickMessage< SICK_LMS_1XX_MSG_HEADER_LEN, SICK_LMS_1XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_1XX_MSG_TRAILER_LEN > {
public:
/** A standard constructor */
SickLMS1xxMessage( );
/** Constructs a packet by using BuildMessage */
SickLMS1xxMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Constructs a packet using ParseMessage() */
SickLMS1xxMessage( const uint8_t * const message_buffer );
/** Construct a well-formed raw packet */
void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Populates fields from a (well-formed) raw packet */
void ParseMessage( const uint8_t * const message_buffer ) throw ( SickIOException );
/** Get the length of the service code associated with the message */
std::string GetCommandType( ) const { return _command_type; }
/** Get the service sub-code associated with the message */
std::string GetCommand( ) const { return _command; }
/** Reset the data associated with this message (for initialization purposes) */
void Clear( );
/** A debugging function that prints the contents of the frame. */
void Print( ) const;
/** Destructor */
~SickLMS1xxMessage( );
private:
/** Command type associated w/ message */
std::string _command_type;
/** Command associated w/ message */
std::string _command;
};
} /* namespace SickToolbox */
#endif /* SICK_LMS_1XX_MESSAGE_HH */

137
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS1xxUtility.hh Normal file
View File

@ -0,0 +1,137 @@
/*!
* \file SickLMS1xxUtility.hh
* \brief Defines simple utility functions for working with the
* Sick LMS 1xx laser range finder units.
*
* Code by Jason C. Derenick and Christopher R. Mansley.
* Contact jasonder(at)seas(dot)upenn(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2009, Jason C. Derenick and Christopher R. Mansley
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include "SickConfig.hh"
/* Implementation Dependencies */
#include <sstream>
/**
* \def REVERSE_BYTE_ORDER_16
* \brief Reverses the byte order of the given 16 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) )
/**
* \def REVERSE_BYTE_ORDER_32
* \brief Reverses the byte order of the given 32 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) )
/* Associate the namespace */
namespace SickToolbox {
#ifndef WORDS_BIGENDIAN
/* NOTE: The following functions are necessary since the Sick LD doesn't adopt the
* convention of converting from network byte order.
*/
/**
* \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint16_t host_to_sick_lms_1xx_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian)
* \param value The 4-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint32_t host_to_sick_lms_1xx_byte_order( uint32_t value ) {
return value;
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint16_t sick_lms_1xx_to_host_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian)
* \param value The 4-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint32_t sick_lms_1xx_to_host_byte_order( uint32_t value ) {
return value;
}
#else // The host has a big-endian architecture
/**
* \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint16_t host_to_sick_lms_1xx_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian)
* \param value The 4-byte value to convertto little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint32_t host_to_sick_lms_1xx_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian)
* \param value The 2-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint16_t sick_lms_1xx_to_host_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian)
* \param value The 4-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint32_t sick_lms_1xx_to_host_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
#endif /* _LITTLE_ENDIAN_HOST */
/*
* NOTE: Other utility functions can be defined here
*/
/**
* \brief Utility function for converting int to standard string
* \param value Integer to be converted
* \return String representing the given integer
*/
inline std::string int_to_str( const int value ) {
std::stringstream ss;
ss << value;
return ss.str();
}
} //namespace SickToolbox

996
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS2xx.hh Normal file
View File

@ -0,0 +1,996 @@
/*!
* \file SickLMS2xx.hh
* \brief Definition of class SickLMS2xx.
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_2XX_HH
#define SICK_LMS_2XX_HH
/* Implementation dependencies */
#include <string>
#include <iostream>
#include <termios.h>
#include "SickLIDAR.hh"
#include "SickException.hh"
#include "SickLMS2xxBufferMonitor.hh"
#include "SickLMS2xxMessage.hh"
/* Macro definitions */
#define DEFAULT_SICK_LMS_2XX_SICK_BAUD (B9600) ///< Initial baud rate of the LMS (whatever is set in flash)
#define DEFAULT_SICK_LMS_2XX_HOST_ADDRESS (0x80) ///< Client/host default serial address
#define DEFAULT_SICK_LMS_2XX_SICK_ADDRESS (0x00) ///< Sick LMS default serial address
#define DEFAULT_SICK_LMS_2XX_SICK_PASSWORD "SICK_LMS" ///< Password for entering installation mode
#define DEFAULT_SICK_LMS_2XX_SICK_MESSAGE_TIMEOUT (unsigned int)(1e6) ///< The max time to wait for a message reply (usecs)
#define DEFAULT_SICK_LMS_2XX_SICK_SWITCH_MODE_TIMEOUT (unsigned int)(3e6) ///< Can take the Sick LD up to 3 seconds to reply (usecs)
#define DEFAULT_SICK_LMS_2XX_SICK_MEAN_VALUES_MESSAGE_TIMEOUT (unsigned int)(15e6) ///< When using mean values, the Sick can sometimes take more than 10s to respond
#define DEFAULT_SICK_LMS_2XX_SICK_CONFIG_MESSAGE_TIMEOUT (unsigned int)(15e6) ///< The sick can take some time to respond to config commands (usecs)
#define DEFAULT_SICK_LMS_2XX_BYTE_INTERVAL (55) ///< Minimum time in microseconds between transmitted bytes
#define DEFAULT_SICK_LMS_2XX_NUM_TRIES (3) ///< The max number of tries before giving up on a request
/* Associate the namespace */
namespace SickToolbox {
/*!
* \brief A general class for interfacing w/ SickLMS2xx2xx laser range finders
*
* This class implements the basic telegram protocol for SickLMS2xx2xx range finders.
* It allows the setting of such parameters as angular resolution, fov, etc...
*/
class SickLMS2xx : public SickLIDAR< SickLMS2xxBufferMonitor, SickLMS2xxMessage >
{
public:
/** Define the maximum number of measurements */
static const uint16_t SICK_MAX_NUM_MEASUREMENTS = 721; ///< Maximum number of measurements returned by the Sick LMS
/*!
* \enum sick_lms_2xx_type_t
* \brief Defines the Sick LMS 2xx types.
* This enum lists all of the supported Sick LMS models.
*/
enum sick_lms_2xx_type_t {
/* Supported 200 models */
SICK_LMS_TYPE_200_30106, ///< Sick LMS type 200-30106
/* Supported 211 models */
SICK_LMS_TYPE_211_30106, ///< Sick LMS type 211-30106
SICK_LMS_TYPE_211_30206, ///< Sick LMS type 211-30206
SICK_LMS_TYPE_211_S07, ///< Sick LMS type 211-S07
SICK_LMS_TYPE_211_S14, ///< Sick LMS type 211-S14
SICK_LMS_TYPE_211_S15, ///< Sick LMS type 211-S15
SICK_LMS_TYPE_211_S19, ///< Sick LMS type 211-S19
SICK_LMS_TYPE_211_S20, ///< Sick LMS type 211-S20
/* Supported 220 models */
SICK_LMS_TYPE_220_30106, ///< Sick LMS type 220-30106
/* Supported 221 models */
SICK_LMS_TYPE_221_30106, ///< Sick LMS type 221-30106
SICK_LMS_TYPE_221_30206, ///< Sick LMS type 221-30206
SICK_LMS_TYPE_221_S07, ///< Sick LMS type 221-S07
SICK_LMS_TYPE_221_S14, ///< Sick LMS type 221-S14
SICK_LMS_TYPE_221_S15, ///< Sick LMS type 221-S15
SICK_LMS_TYPE_221_S16, ///< Sick LMS type 221-S16
SICK_LMS_TYPE_221_S19, ///< Sick LMS type 221-S19
SICK_LMS_TYPE_221_S20, ///< Sick LMS type 221-S20
/* Supported 291 models */
SICK_LMS_TYPE_291_S05, ///< Sick LMS type 291-S05
SICK_LMS_TYPE_291_S14, ///< Sick LMS type 291-S14 (LMS Fast)
SICK_LMS_TYPE_291_S15, ///< Sick LMS type 291-S15
/* Any unknown model */
SICK_LMS_TYPE_UNKNOWN = 0xFF ///< Unknown sick type
};
/*!
* \enum sick_lms_2xx_variant_t
* \brief Defines the Sick LMS 2xx variant type.
*/
enum sick_lms_2xx_variant_t {
SICK_LMS_VARIANT_2XX_TYPE_6 = 0x00, ///< Standard LMS 2xx type 6 models
SICK_LMS_VARIANT_SPECIAL = 0x01, ///< Special models (i.e. LMS211-/221-S19/-S20
SICK_LMS_VARIANT_UNKNOWN = 0xFF ///< Unknown LMS variant
};
/*!
* \enum sick_lms_2xx_scan_angle_t
* \brief Defines the scan angle for the Sick LMS 2xx.
*/
enum sick_lms_2xx_scan_angle_t {
SICK_SCAN_ANGLE_90 = 90, ///< Scanning angle of 90 degrees
SICK_SCAN_ANGLE_100 = 100, ///< Scanning angle of 100 degrees
SICK_SCAN_ANGLE_180 = 180, ///< Scanning angle of 180 degrees
SICK_SCAN_ANGLE_UNKNOWN = 0xFF ///< Unknown scanning angle
};
/*!
* \enum sick_lms_2xx_scan_resolution_t
* \brief Defines the available resolution settings for the Sick LMS 2xx.
*/
enum sick_lms_2xx_scan_resolution_t {
SICK_SCAN_RESOLUTION_25 = 25, ///< 0.25 degree angular resolution
SICK_SCAN_RESOLUTION_50 = 50, ///< 0.50 degree angular resolution
SICK_SCAN_RESOLUTION_100 = 100, ///< 1.00 degree angular resolution
SICK_SCAN_RESOLUTION_UNKNOWN = 0xFF ///< Unknown angular resolution
};
/*!
* \enum sick_lms_2xx_measuring_units_t
* \brief Defines the available Sick LMS 2xx measured value units.
*/
enum sick_lms_2xx_measuring_units_t {
SICK_MEASURING_UNITS_CM = 0x00, ///< Measured values are in centimeters
SICK_MEASURING_UNITS_MM = 0x01, ///< Measured values are in milimeters
SICK_MEASURING_UNITS_UNKNOWN = 0xFF ///< Unknown units
};
/*!
* \enum sick_lms_2xx_sensitivity_t
* \brief Sick sensitivities. Only valid for Sick LMS 211/221/291!
*/
enum sick_lms_2xx_sensitivity_t {
SICK_SENSITIVITY_STANDARD = 0x00, ///< Standard sensitivity: 30m @ 10% reflectivity
SICK_SENSITIVITY_MEDIUM = 0x01, ///< Medium sensitivity: 25m @ 10% reflectivity
SICK_SENSITIVITY_LOW = 0x02, ///< Low sensitivity: 20m @ 10% reflectivity
SICK_SENSITIVITY_HIGH = 0x03, ///< High sensitivity: 42m @ 10% reflectivity
SICK_SENSITIVITY_UNKNOWN = 0xFF ///< Sensitivity unknown
};
/*!
* \enum sick_lms_2xx_peak_threshold_t
* \brief Sick peak threshold. Only valid for Sick LMS 200/220!
*/
enum sick_lms_2xx_peak_threshold_t {
SICK_PEAK_THRESHOLD_DETECTION_WITH_NO_BLACK_EXTENSION = 0x00, ///< Standard: peak threshold detection, no black extension
SICK_PEAK_THRESHOLD_DETECTION_WITH_BLACK_EXTENSION = 0x01, ///< Peak threshold detection, active black extension
SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_NO_BLACK_EXTENSION = 0x02, ///< No peak threshold detection, no black extension
SICK_PEAK_THRESHOLD_NO_DETECTION_WITH_BLACK_EXTENSION = 0x03, ///< No peak threshold detection, active black extension
SICK_PEAK_THRESHOLD_UNKNOWN = 0xFF ///< Peak threshold unknown
};
/*!
* \enum sick_lms_2xx_status_t
* \brief Defines the status of the Sick LMS 2xx unit.
*/
enum sick_lms_2xx_status_t {
SICK_STATUS_OK = 0x00, ///< LMS is OK
SICK_STATUS_ERROR = 0x01, ///< LMS has encountered an error
SICK_STATUS_UNKNOWN = 0xFF ///< Unknown LMS status
};
/*!
* \enum sick_lms_2xx_measuring_mode_t
* \brief Defines the measurment modes supported by Sick LMS 2xx.
*/
enum sick_lms_2xx_measuring_mode_t {
SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE = 0x00, ///< Measurement range 8m/80m; fields A,B and Dazzle (Default)
SICK_MS_MODE_8_OR_80_REFLECTOR = 0x01, ///< Measurement range 8/80m; reflector bits in 8 levels
SICK_MS_MODE_8_OR_80_FA_FB_FC = 0x02, ///< Measurement range 8/80m; fields A,B, and C
SICK_MS_MODE_16_REFLECTOR = 0x03, ///< Measurement range 16m; reflector bits in 4 levels
SICK_MS_MODE_16_FA_FB = 0x04, ///< Measurement range 16m; fields A and B
SICK_MS_MODE_32_REFLECTOR = 0x05, ///< Measurement range 32m; reflector bit in 2 levels
SICK_MS_MODE_32_FA = 0x06, ///< Measurement range 32m; field A
SICK_MS_MODE_32_IMMEDIATE = 0x0F, ///< Measurement range 32m; immediate data transmission, no flags
SICK_MS_MODE_REFLECTIVITY = 0x3F, ///< Sick LMS 2xx returns reflectivity (echo amplitude) values instead of range measurements
SICK_MS_MODE_UNKNOWN = 0xFF ///< Unknown range
};
/*!
* \enum sick_lms_2xx_operating_mode_t
* \brief Defines the operating modes supported by Sick LMS 2xx.
* See page 41 of the LMS 2xx telegram manual for additional descriptions of these modes.
*/
enum sick_lms_2xx_operating_mode_t {
SICK_OP_MODE_INSTALLATION = 0x00, ///< Installation mode for writing EEPROM
SICK_OP_MODE_DIAGNOSTIC = 0x10, ///< Diagnostic mode for testing purposes
SICK_OP_MODE_MONITOR_STREAM_MIN_VALUE_FOR_EACH_SEGMENT = 0x20, ///< Streams minimum measured values for each segement
SICK_OP_MODE_MONITOR_TRIGGER_MIN_VALUE_ON_OBJECT = 0x21, ///< Sends the min measured values when object is detected
SICK_OP_MODE_MONITOR_STREAM_MIN_VERT_DIST_TO_OBJECT = 0x22, ///< Streams min "vertical distance" to objects
SICK_OP_MODE_MONITOR_TRIGGER_MIN_VERT_DIST_TO_OBJECT = 0x23, ///< Sends min vertical distance to object when detected
SICK_OP_MODE_MONITOR_STREAM_VALUES = 0x24, ///< Streams all measured values in a scan
SICK_OP_MODE_MONITOR_REQUEST_VALUES = 0x25, ///< Sends measured range values on request (i.e. when polled)
SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES = 0x26, ///< Streams mean values from a sample size of n consecutive scans
SICK_OP_MODE_MONITOR_STREAM_VALUES_SUBRANGE = 0x27, ///< Streams data from given subrange
SICK_OP_MODE_MONITOR_STREAM_MEAN_VALUES_SUBRANGE = 0x28, ///< Streams mean values over requested subrange
SICK_OP_MODE_MONITOR_STREAM_VALUES_WITH_FIELDS = 0x29, ///< Streams measured values with associated flags
SICK_OP_MODE_MONITOR_STREAM_VALUES_FROM_PARTIAL_SCAN = 0x2A, ///< Streams measured values of partial scan directly after measurement
SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT_FROM_PARTIAL_SCAN = 0x2B, ///< Streams range and intensity from n partial scans
SICK_OP_MODE_MONITOR_STREAM_MIN_VALUES_FOR_EACH_SEGMENT_SUBRANGE = 0x2C, ///< Streams minimum measured values for each segment in a sub-range
SICK_OP_MODE_MONITOR_NAVIGATION = 0x2E, ///< Sick outputs navigation data records
SICK_OP_MODE_MONITOR_STREAM_RANGE_AND_REFLECT = 0x50, ///< Streams measured range from a scan and sub-range of reflectivity values
SICK_OP_MODE_UNKNOWN = 0xFF ///< Unknown operating mode
};
/*!
* \enum sick_lms_2xx_baud_t
* \brief Defines available Sick LMS 2xx baud rates
*/
enum sick_lms_2xx_baud_t {
SICK_BAUD_9600 = 0x42, ///< 9600 baud
SICK_BAUD_19200 = 0x41, ///< 19200 baud
SICK_BAUD_38400 = 0x40, ///< 38400 baud
SICK_BAUD_500K = 0x48, ///< 500000 baud
SICK_BAUD_UNKNOWN = 0xFF ///< Unknown baud rate
};
/** Define Sick LMS 2xx availability levels */
static const uint8_t SICK_FLAG_AVAILABILITY_DEFAULT = 0x00; ///< Availability unspecified
static const uint8_t SICK_FLAG_AVAILABILITY_HIGH = 0x01; ///< Highest availability (comparable to LMS types 1 to 5)
static const uint8_t SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES = 0x02; ///< Send real-time indices
static const uint8_t SICK_FLAG_AVAILABILITY_DAZZLE_NO_EFFECT = 0x04; ///< Dazzle evalutation has no effect on switching outputs
/*!
* \struct sick_lms_2xx_operating_status_tag
* \brief A structure for aggregating the data that
* collectively defines the operating status
* of the device.
*/
/*!
* \typedef sick_lms_2xx_operating_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_operating_status_tag {
uint16_t sick_scan_angle; ///< Sick scanning angle (deg)
uint16_t sick_scan_resolution; ///< Sick angular resolution (1/100 deg)
uint16_t sick_num_motor_revs; ///< Sick number of motor revs
uint8_t sick_operating_mode; ///< Sick operating mode
uint8_t sick_measuring_mode; ///< Sick measuring mode
uint8_t sick_laser_mode; ///< Sick laser is on/off
uint8_t sick_device_status; ///< Sick device status {ok,error}
uint8_t sick_measuring_units; ///< Sick measuring units {cm,mm}
uint8_t sick_address; ///< Sick device address
uint8_t sick_variant; ///< Sick variant {special,standard}
} sick_lms_2xx_operating_status_t;
/*!
* \struct sick_lms_2xx_software_status_tag
* \brief A structure for aggregating the data that
* collectively defines the system software
* for the Sick LMS 2xx unit.
*/
/*!
* \typedef sick_lms_2xx_software_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_software_status_tag {
uint8_t sick_system_software_version[8]; ///< Sick system software version
uint8_t sick_prom_software_version[8]; ///< Sick boot prom software version
} sick_lms_2xx_software_status_t;
/*!
* \struct sick_lms_2xx_restart_status_tag
* \brief A structure for aggregating the data that
* collectively defines the system restart
* config for the Sick LMS 2xx unit
*/
typedef struct sick_lms_2xx_restart_status_tag {
uint16_t sick_restart_time; ///< Sick restart time
uint8_t sick_restart_mode; ///< Sick restart mode
} sick_lms_2xx_restart_status_t;
/*!
* \struct sick_lms_2xx_pollution_status_tag
* \brief A structure for aggregating the data that
* collectively defines the pollution values
* and settings for the device
*/
/*!
* \typedef sick_lms_2xx_pollution_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_pollution_status_tag {
uint16_t sick_pollution_vals[8]; ///< Calibrating the pollution channels
uint16_t sick_pollution_calibration_vals[8]; ///< Calibrating the pollution channel values
uint16_t sick_reference_pollution_vals[4]; ///< Reference pollution values
uint16_t sick_reference_pollution_calibration_vals[4]; ///< Reference pollution calibration values
} sick_lms_2xx_pollution_status_t;
/*!
* \struct sick_lms_2xx_signal_status_tag
* \brief A structure for aggregating the data that
* collectively define the signal config and
* status.
*/
/*!
* \typedef sick_lms_2xx_signal_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_signal_status_tag {
uint16_t sick_reference_scale_1_dark_100; ///< Receive signal amplitude in ADC incs when reference signal is switched off (Signal 1, Dark 100%)
uint16_t sick_reference_scale_2_dark_100; ///< Receive signal amplitude in ADC incs when reference signal is switched off (Signal 2, Dark 100%)
uint16_t sick_reference_scale_1_dark_66; ///< Receive signal amplitude in ADC incs when reference signal is switched off (Signal 1, Dark 66%)
uint16_t sick_reference_scale_2_dark_66; ///< Receive signal amplitude in ADC incs when reference signal is switched off (Signal 2, Dark 66%)
uint16_t sick_signal_amplitude; ///< Laser power in % of calibration value
uint16_t sick_current_angle; ///< Angle used for power measurement
uint16_t sick_peak_threshold; ///< Peak threshold in ADC incs for power measurement
uint16_t sick_angle_of_measurement; ///< Angles used to reference target for power measurement
uint16_t sick_signal_amplitude_calibration_val; ///< Calibration of the laser power
uint16_t sick_stop_threshold_target_value; ///< Target value of the stop threshold in ADC incs
uint16_t sick_peak_threshold_target_value; ///< Target value of the peak threshold in ADC incs
uint16_t sick_stop_threshold_actual_value; ///< Actual value of the stop threshold in ADC incs
uint16_t sick_peak_threshold_actual_value; ///< Actual value of the peak threshold in ADC incs
uint16_t sick_reference_target_single_measured_vals; ///< Reference target "single measured values." Low byte: Current number of filtered single measured values. High byte: Max num filtered single measured value since power-on.
uint16_t sick_reference_target_mean_measured_vals; ///< Reference target "mean measured values." Low byte: Current number of filtered mean measured values. High byte: Max num filtered mean measured value since power-on.
} sick_lms_2xx_signal_status_t;
/*!
* \struct sick_lms_2xx_field_status_tag
* \brief A structure for aggregating the data that
* collectively define the signal config and
* status.
*/
/*!
* \typedef sick_lms_2xx_field_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_field_status_tag {
uint8_t sick_field_evaluation_number; ///< Number of evaluations when the field is infirnged (lies in [1,125])
uint8_t sick_field_set_number; ///< Active field set number
uint8_t sick_multiple_evaluation_offset_field_2; ///< Offset for multiple evaluation of field set 2 (see page 105 of telegram listing)
} sick_lms_2xx_field_status_t;
/*!
* \struct sick_lms_2xx_baud_status_tag
* \brief A structure for aggregating the data that
* collectively define the baud config.
*/
/*!
* \typedef sick_lms_2xx_baud_status_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_baud_status_tag {
uint16_t sick_baud_rate; ///< Sick baud as reported by the device
uint8_t sick_permanent_baud_rate; ///< 0 - When power is switched on baud rate is 9600/1 - configured transmission rate is used
} sick_lms_2xx_baud_status_t;
/*!
* \struct sick_lms_2xx_device_config_tag
* \brief A structure for aggregating the data that
* collectively defines the Sick's config.
*/
/*!
* \typedef sick_lms_2xx_device_config_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_device_config_tag {
uint16_t sick_blanking; ///< Maximum diameter of objects that are not to be detected (units cm)
uint16_t sick_fields_b_c_restart_times; ///< Restart times for fields B and C
uint16_t sick_dazzling_multiple_evaluation; ///< Number of scans that take place before LMS switches the outputs (only applies to availability level 1)
uint8_t sick_peak_threshold; ///< Peak threshold/black correction (This applies to Sick LMS 200/220 models, when Sick LMS 211/221/291 models are used, this value is sensitivity)
uint8_t sick_stop_threshold; ///< Stop threshold in mV (This only applies to Sick LMS 200/220 models)
uint8_t sick_availability_level; ///< Availability level of the Sick LMS
uint8_t sick_measuring_mode; ///< Measuring mode of the device
uint8_t sick_measuring_units; ///< Measured value and field value units
uint8_t sick_temporary_field; ///< Indicates whether the temporary field is being used
uint8_t sick_subtractive_fields; ///< Indicates whether fields A and B are subtractive
uint8_t sick_multiple_evaluation; ///< Multiple evalutation of scan data
uint8_t sick_restart; ///< Indicates the restart level of the device
uint8_t sick_restart_time; ///< Inidicates the restart time of the device
uint8_t sick_multiple_evaluation_suppressed_objects; ///< Multiple evaluation for objects less than the blanking size
uint8_t sick_contour_a_reference; ///< Contour function A
uint8_t sick_contour_a_positive_tolerance_band; ///< When contour function is active the positive tolerance is defined in (cm)
uint8_t sick_contour_a_negative_tolerance_band; ///< When contour function is active the negative tolerance is defined in (cm)
uint8_t sick_contour_a_start_angle; ///< When contour function is active the start angle of area to be monitored is defined (deg)
uint8_t sick_contour_a_stop_angle; ///< When contour function is active the stop angle of area to be monitored is defined (deg)
uint8_t sick_contour_b_reference; ///< Contour function B
uint8_t sick_contour_b_positive_tolerance_band; ///< When contour function is active the positive tolerance is defined in (cm)
uint8_t sick_contour_b_negative_tolerance_band; ///< When contour function is active the negative tolerance is defined in (cm) uint8_t sick_contour_b_start_angle; ///< When contour function is active the start angle of area to be monitored is defined (deg)
uint8_t sick_contour_b_start_angle; ///< When contour function is active the start angle of area to be monitored is defined (deg)
uint8_t sick_contour_b_stop_angle; ///< When contour function is active the stop angle of area to be monitored is defined (deg)
uint8_t sick_contour_c_reference; ///< Contour function C
uint8_t sick_contour_c_positive_tolerance_band; ///< When contour function is active the positive tolerance is defined in (cm)
uint8_t sick_contour_c_negative_tolerance_band; ///< When contour function is active the negative tolerance is defined in (cm)
uint8_t sick_contour_c_start_angle; ///< When contour function is active the start angle of area to be monitored is defined (deg)
uint8_t sick_contour_c_stop_angle; ///< When contour function is active the stop angle of area to be monitored is defined (deg)
uint8_t sick_pixel_oriented_evaluation; ///< Pixel oriented evaluation
uint8_t sick_single_measured_value_evaluation_mode; ///< Multiple evaluation (min: 1, max: 125)
} sick_lms_2xx_device_config_t;
/*!
* \struct sick_lms_2xx_scan_profile_b0_tag
* \brief A structure for aggregating the data that
* define a scan profile obtained from reply
* B0 (See page 49 Telegram listing)
*/
/*!
* \typedef sick_lms_2xx_scan_profile_b0_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_scan_profile_b0_tag {
uint16_t sick_num_measurements; ///< Number of measurements
uint16_t sick_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Range/reflectivity measurement buffer
uint8_t sick_field_a_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field A bit value returned w/ range measurement
uint8_t sick_field_b_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field B but value returned w/ range measurement
uint8_t sick_field_c_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field C (or dazzle - depending upon sensor mode) value returned w/ range measurement
uint8_t sick_telegram_index; ///< Telegram index modulo 256
uint8_t sick_real_time_scan_index; ///< If real-time scan indices are requested, this value is set (modulo 256)
uint8_t sick_partial_scan_index; ///< Indicates the start angle of the scan (This is useful for partial scans)
} sick_lms_2xx_scan_profile_b0_t;
/*!
* \struct sick_lms_2xx_scan_profile_b6_tag
* \brief A structure for aggregating the data that
* define a scan profile obtained from reply
* B6 (See page 61 Telegram listing)
*/
/*!
* \typedef sick_lms_2xx_scan_profile_b6_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_scan_profile_b6_tag {
uint16_t sick_num_measurements; ///< Number of measurements
uint16_t sick_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Range/reflectivity measurement buffer
uint8_t sick_sample_size; ///< Number of scans used in computing the returned mean
uint8_t sick_telegram_index; ///< Telegram index modulo 256
uint8_t sick_real_time_scan_index; ///< If real-time scan indices are requested, this value is set (modulo 256)
} sick_lms_2xx_scan_profile_b6_t;
/*!
* \struct sick_lms_2xx_scan_profile_b7_tag
* \brief A structure for aggregating the data that
* define a scan profile obtained from reply
* B7 (See page 63 Telegram listing)
*/
/*!
* \typedef sick_lms_2xx_scan_profile_b7_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_scan_profile_b7_tag {
uint16_t sick_subrange_start_index; ///< Measurement subrange start index
uint16_t sick_subrange_stop_index; ///< Measurement subrange stop index
uint16_t sick_num_measurements; ///< Number of measurements
uint16_t sick_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Range/reflectivity measurement buffer
uint8_t sick_field_a_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field A bit value returned w/ range measurement
uint8_t sick_field_b_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field B but value returned w/ range measurement
uint8_t sick_field_c_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field C (or dazzle - depending upon sensor mode) value returned w/ range measurement
uint8_t sick_telegram_index; ///< Telegram index modulo 256
uint8_t sick_real_time_scan_index; ///< If real-time scan indices are requested, this value is set (modulo 256)
uint8_t sick_partial_scan_index; ///< Indicates the start angle of the scan (This is useful for partial scans)
} sick_lms_2xx_scan_profile_b7_t;
/*!
* \struct sick_lms_2xx_scan_profile_bf_tag
* \brief A structure for aggregating the data that
* define a scan profile obtained from reply
* BF (See page 71 Telegram listing)
*/
/*!
* \typedef sick_lms_2xx_scan_profile_bf_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_scan_profile_bf_tag {
uint16_t sick_subrange_start_index; ///< Measurement subrange start index
uint16_t sick_subrange_stop_index; ///< Measurement subrange stop index
uint16_t sick_num_measurements; ///< Number of measurements
uint16_t sick_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Range/reflectivity measurement buffer
uint8_t sick_sample_size; ///< Number of scans used in computing the returned mean
uint8_t sick_telegram_index; ///< Telegram index modulo 256
uint8_t sick_real_time_scan_index; ///< If real-time scan indices are requested, this value is set (modulo 256)
} sick_lms_2xx_scan_profile_bf_t;
/*!
* \struct sick_lms_2xx_scan_profile_c4_tag
* \brief A structure for aggregating the data that
* define a scan profile obtained from reply
* B4 (See page 79 Telegram listing)
*/
/*!
* \typedef sick_lms_2xx_scan_profile_c4_t
* \brief Adopt c-style convention
*/
typedef struct sick_lms_2xx_scan_profile_c4_tag {
uint16_t sick_num_range_measurements; ///< Number of range measurements
uint16_t sick_num_reflect_measurements; ///< Number of reflectivity measurements
uint16_t sick_range_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Range measurement buffer
uint16_t sick_reflect_measurements[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflect measurements buffer
uint16_t sick_reflect_subrange_start_index; ///< Start index of the measured reflectivity value subrange
uint16_t sick_reflect_subrange_stop_index; ///< Stop index of the measured reflectivity value subrange
uint8_t sick_field_a_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field A bit value returned w/ range measurement
uint8_t sick_field_b_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field B but value returned w/ range measurement
uint8_t sick_field_c_values[SICK_MAX_NUM_MEASUREMENTS]; ///< Reflects the Field C (or dazzle - depending upon sensor mode) value returned w/ range measurement
uint8_t sick_telegram_index; ///< Telegram index modulo 256
uint8_t sick_real_time_scan_index; ///< If real-time scan indices are requested, this value is set (modulo 256)
} sick_lms_2xx_scan_profile_c4_t;
/** Constructor */
SickLMS2xx( const std::string sick_device_path );
/** Destructor */
~SickLMS2xx( );
/** Initializes the Sick */
void Initialize( const sick_lms_2xx_baud_t desired_baud_rate, const uint32_t delay = 0 )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Uninitializes the Sick */
void Uninitialize( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Gets the Sick LMS 2xx device path */
std::string GetSickDevicePath( ) const;
/** Gets the Sick LMS 2xx device type */
sick_lms_2xx_type_t GetSickType( ) const throw( SickConfigException );
/** Gets the scan angle currently being used by the device */
double GetSickScanAngle( ) const throw( SickConfigException );
/** Gets the scan resolution currently being used by the device */
double GetSickScanResolution( ) const throw( SickConfigException );
/** Set the measurement units of the device (in EEPROM) */
void SetSickMeasuringUnits( const sick_lms_2xx_measuring_units_t sick_units = SICK_MEASURING_UNITS_MM )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Get the current measurement units of the device */
SickLMS2xx::sick_lms_2xx_measuring_units_t GetSickMeasuringUnits( ) const throw( SickConfigException );
/** Sets the sensitivity value for the device (in EEPROM). NOTE: Only applies to LMS 211/221/291 models. */
void SetSickSensitivity( const sick_lms_2xx_sensitivity_t sick_sensitivity = SICK_SENSITIVITY_STANDARD )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Get the current Sick LMS 2xx sensitivity level. NOTE: Only applies to LMS 211/221/291 models. */
sick_lms_2xx_sensitivity_t GetSickSensitivity( ) const throw( SickConfigException );
/** Sets the peak threshold mode for the device (in EEPROM). NOTE: Only applies to LMS 200/220 models */
void SetSickPeakThreshold( const sick_lms_2xx_peak_threshold_t sick_peak_threshold = SICK_PEAK_THRESHOLD_DETECTION_WITH_NO_BLACK_EXTENSION )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Get the current Sick LMS 2xx sensitivity level. NOTE: Only applies to LMS 211/221/291 models. */
sick_lms_2xx_peak_threshold_t GetSickPeakThreshold( ) const throw( SickConfigException );;
/** Sets the measuring mode for the device (in EEPROM). See page 98 of the telegram listing for more details. */
void SetSickMeasuringMode( const sick_lms_2xx_measuring_mode_t sick_measuring_mode = SICK_MS_MODE_8_OR_80_FA_FB_DAZZLE )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Get the current Sick LMS 2xx measuring mode */
sick_lms_2xx_measuring_mode_t GetSickMeasuringMode( ) const throw( SickConfigException );
/** Get the current Sick LMS 2xx operating mode */
sick_lms_2xx_operating_mode_t GetSickOperatingMode( ) const throw( SickConfigException );
/** Sets the availability of the device (in EEPROM). See page 98 of the telegram listing for more details. */
void SetSickAvailability( const uint8_t sick_availability_flags = SICK_FLAG_AVAILABILITY_DEFAULT )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Gets the current availability flags for the device */
uint8_t GetSickAvailability( ) const throw( SickConfigException );
/** Sets the variant type for the device (in EEPROM) */
void SetSickVariant( const sick_lms_2xx_scan_angle_t scan_angle, const sick_lms_2xx_scan_resolution_t scan_resolution )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Gets measurement data from the Sick. NOTE: Data can be either range or reflectivity given the Sick mode. */
void GetSickScan( unsigned int * const measurement_values,
unsigned int & num_measurement_values,
unsigned int * const sick_field_a_values = NULL,
unsigned int * const sick_field_b_values = NULL,
unsigned int * const sick_field_c_values = NULL,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_scan_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Gets range and reflectivity data from the Sick. NOTE: This only applies to Sick LMS 211/221/291-S14! */
void GetSickScan( unsigned int * const range_values,
unsigned int * const reflect_values,
unsigned int & num_range_measurements,
unsigned int & num_reflect_measurements,
unsigned int * const sick_field_a_values = NULL,
unsigned int * const sick_field_b_values = NULL,
unsigned int * const sick_field_c_values = NULL,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_scan_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Gets measurement data from the Sick. NOTE: Data can be either range or reflectivity given the Sick mode. */
void GetSickScanSubrange( const uint16_t sick_subrange_start_index,
const uint16_t sick_subrange_stop_index,
unsigned int * const measurement_values,
unsigned int & num_measurement_values,
unsigned int * const sick_field_a_values = NULL,
unsigned int * const sick_field_b_values = NULL,
unsigned int * const sick_field_c_values = NULL,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_scan_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Gets partial scan measurements from the Sick LMS 2xx. NOTE: Data can be either range or reflectivity depending upon the given Sick mode. */
void GetSickPartialScan( unsigned int * const measurement_values,
unsigned int & num_measurement_values,
unsigned int & partial_scan_index,
unsigned int * const sick_field_a_values = NULL,
unsigned int * const sick_field_b_values = NULL,
unsigned int * const sick_field_c_values = NULL,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_scan_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Gets mean measured values from the Sick LMS */
void GetSickMeanValues( const uint8_t sick_sample_size,
unsigned int * const measurement_values,
unsigned int & num_measurement_values,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Gets mean measured values from the Sick LMS */
void GetSickMeanValuesSubrange( const uint8_t sick_sample_size,
const uint16_t sick_subrange_start_index,
const uint16_t sick_subrange_stop_index,
unsigned int * const measurement_values,
unsigned int & num_measurement_values,
unsigned int * const sick_telegram_index = NULL,
unsigned int * const sick_real_time_index = NULL ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Acquire the Sick LMS status */
sick_lms_2xx_status_t GetSickStatus( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Indicates whether the Sick is an LMS Fast */
bool IsSickLMS2xxFast( ) const throw( SickConfigException );
/** Resets Sick LMS field values */
void ResetSick( ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Get Sick status as a string */
std::string GetSickStatusAsString( ) const;
/** Get Sick software info as a string */
std::string GetSickSoftwareVersionAsString( ) const;
/** Get Sick config as a string */
std::string GetSickConfigAsString( ) const;
/** Print the Sick LMS status */
void PrintSickStatus( ) const;
/** Print the Sick LMS software versions */
void PrintSickSoftwareVersion( ) const;
/** Print the Sick LMS configuration */
void PrintSickConfig( ) const;
/*
* NOTE: The following methods are given to make working with our
* predefined types a bit more manageable.
*/
/** Converts the LMS's type to a corresponding string */
static std::string SickTypeToString( const sick_lms_2xx_type_t sick_type );
/** A utility function for converting integers to lms_sick_scan_angle_t */
static sick_lms_2xx_scan_angle_t IntToSickScanAngle( const int scan_angle_int );
/** A utility function for converting ints to lms_sick_scan_resolution_t */
static sick_lms_2xx_scan_resolution_t IntToSickScanResolution( const int scan_resolution_int );
/** A utility function for converting doubles to lms_sick_scan_resolution_t */
static sick_lms_2xx_scan_resolution_t DoubleToSickScanResolution( const double scan_resolution_double );
/** Converts the given bad, returns a string representing that baud rate. */
static std::string SickBaudToString( const sick_lms_2xx_baud_t baud_rate );
/** A utility function for converting integers to lms_baud_t */
static sick_lms_2xx_baud_t IntToSickBaud( const int baud_int );
/** A utility function for converting baud strings to lms_baud_t */
static sick_lms_2xx_baud_t StringToSickBaud( const std::string baud_str );
/** Converts the LMS's status to a corresponding string */
static std::string SickStatusToString( const sick_lms_2xx_status_t sick_status );
/** Converts the LMS's measuring mode to a corresponding string */
static std::string SickMeasuringModeToString( const sick_lms_2xx_measuring_mode_t sick_measuring_mode );
/** Converts the LMS's measuring mode to a corresponding string */
static std::string SickOperatingModeToString( const sick_lms_2xx_operating_mode_t sick_operating_mode );
/** Converts the LMS's sensitivity to string */
static std::string SickSensitivityToString( const sick_lms_2xx_sensitivity_t sick_sensitivity );
/** Converts the LMS's peak threshold to string */
static std::string SickPeakThresholdToString( const sick_lms_2xx_peak_threshold_t sick_peak_threshold );
/** Converts the LMS's measuring units to a corresponding string */
static std::string SickMeasuringUnitsToString( const sick_lms_2xx_measuring_units_t sick_units );
protected:
/** A path to the device at which the sick can be accessed. */
std::string _sick_device_path;
/** The baud rate at which to communicate with the Sick */
sick_lms_2xx_baud_t _curr_session_baud;
/** The desired baud rate for communicating w/ the Sick */
sick_lms_2xx_baud_t _desired_session_baud;
/** A string representing the type of device */
sick_lms_2xx_type_t _sick_type;
/** The operating parameters of the device */
sick_lms_2xx_operating_status_t _sick_operating_status;
/** The current software version being run on the device */
sick_lms_2xx_software_status_t _sick_software_status;
/** The restart configuration of the device */
sick_lms_2xx_restart_status_t _sick_restart_status;
/** The pollution measurement status */
sick_lms_2xx_pollution_status_t _sick_pollution_status;
/** The signal status of the device */
sick_lms_2xx_signal_status_t _sick_signal_status;
/** The field configuration for the device */
sick_lms_2xx_field_status_t _sick_field_status;
/** The baud configuration of the device */
sick_lms_2xx_baud_status_t _sick_baud_status;
/** The device configuration for the Sick */
sick_lms_2xx_device_config_t _sick_device_config;
/** Used when the device is streaming mean values */
uint8_t _sick_mean_value_sample_size;
/** Used when the device is streaming a scan subrange */
uint16_t _sick_values_subrange_start_index;
/** Used when the device is streaming a scan subrange */
uint16_t _sick_values_subrange_stop_index;
/** Stores information about the original terminal settings */
struct termios _old_term;
/** Opens the terminal for serial communication. */
void _setupConnection() throw( SickIOException, SickThreadException );
void _setupConnection(const uint32_t delay ) throw( SickIOException, SickThreadException );
/** Closes the serial communication terminal. */
void _teardownConnection( ) throw( SickIOException );
/** Sends a message to the LMS and get the expected reply using th 0x80 rule. @todo Check difference in comments? */
void _sendMessageAndGetReply( const SickLMS2xxMessage &sick_send_message,
SickLMS2xxMessage &sick_recv_message,
const unsigned int timeout_value,
const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException );
/** Sends a message to the LMS and get the expected reply using th 0x80 rule. @todo Check difference in comments? */
void _sendMessageAndGetReply( const SickLMS2xxMessage &sick_send_message,
SickLMS2xxMessage &sick_recv_message,
const uint8_t reply_code,
const unsigned int timeout_value,
const unsigned int num_tries ) throw( SickIOException, SickThreadException, SickTimeoutException );
/** Flushes the terminal I/O buffers */
void _flushTerminalBuffer( ) throw ( SickThreadException );
/** Sets the baud rate for communication with the LMS. */
void _setSessionBaud( const sick_lms_2xx_baud_t baud_rate ) throw( SickIOException, SickThreadException, SickTimeoutException );
/** Tests communication wit the LMS at a particular baud rate. */
bool _testSickBaud( const sick_lms_2xx_baud_t baud_rate ) throw( SickIOException, SickThreadException );
/** Changes the terminal's baud rate. */
void _setTerminalBaud( const sick_lms_2xx_baud_t sick_baud ) throw( SickIOException, SickThreadException );
/** Gets the type of Sick LMS */
void _getSickType( ) throw( SickTimeoutException, SickIOException, SickThreadException );
/** Gets the current Sick configuration settings */
void _getSickConfig( ) throw( SickTimeoutException, SickIOException, SickThreadException );
/** Sets the Sick configuration in flash */
void _setSickConfig( const sick_lms_2xx_device_config_t &sick_config ) throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException );
/** Gets the status of the LMS */
void _getSickStatus( ) throw( SickTimeoutException, SickIOException, SickThreadException );
/** Gets the error status of the Sick LMS */
void _getSickErrors( unsigned int * const num_sick_errors = NULL,
uint8_t * const error_type_buffer = NULL,
uint8_t * const error_num_buffer = NULL ) throw( SickTimeoutException, SickIOException, SickThreadException );
/** Switch Sick LMS to installation mode */
void _setSickOpModeInstallation( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to diagnostic mode */
void _setSickOpModeDiagnostic( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (request range data) */
void _setSickOpModeMonitorRequestValues( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream range) */
void _setSickOpModeMonitorStreamValues( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream range and reflectivity) */
void _setSickOpModeMonitorStreamRangeAndReflectivity( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream range from a partial scan) */
void _setSickOpModeMonitorStreamValuesFromPartialScan( )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream mean measured values) */
void _setSickOpModeMonitorStreamMeanValues( const uint8_t sample_size )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream mean measured values) */
void _setSickOpModeMonitorStreamValuesSubrange( const uint16_t subrange_start_index, const uint16_t subrange_stop_index )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switch Sick LMS to monitor mode (stream mean measured values subrange) */
void _setSickOpModeMonitorStreamMeanValuesSubrange( const uint16_t sample_size, const uint16_t subrange_start_index, const uint16_t subrange_stop_index )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Switches the operating mode of the LMS. */
void _switchSickOperatingMode( const uint8_t sick_mode, const uint8_t * const mode_params = NULL )
throw( SickConfigException, SickTimeoutException, SickIOException, SickThreadException);
/** Parses the scan profile returned w/ message B0 */
void _parseSickScanProfileB0( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b0_t &sick_scan_profile ) const;
/** Parses the scan profile returned w/ message B6 */
void _parseSickScanProfileB6( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b6_t &sick_scan_profile ) const;
/** Parses the scan profile returned w/ message B6 */
void _parseSickScanProfileB7( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_b7_t &sick_scan_profile ) const;
/** Parses the scan profile returned w/ message BF */
void _parseSickScanProfileBF( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_bf_t &sick_scan_profile ) const;
/** Parses the scan profile returned w/ message C4 */
void _parseSickScanProfileC4( const uint8_t * const src_buffer, sick_lms_2xx_scan_profile_c4_t &sick_scan_profile ) const;
/** A function for parsing a byte sequence into a device config structure */
void _parseSickConfigProfile( const uint8_t * const src_buffer, sick_lms_2xx_device_config_t &sick_device_config ) const;
/** Acquires the bit mask to extract the field bit values returned with each range measurement */
void _extractSickMeasurementValues( const uint8_t * const byte_sequence, const uint16_t num_measurements, uint16_t * const measured_values,
uint8_t * const field_a_values = NULL, uint8_t * const field_b_values = NULL, uint8_t * const field_c_values = NULL ) const;
/** Tells whether the device is returning real-time indices */
bool _returningRealTimeIndices( ) const { return _sick_device_config.sick_availability_level & SICK_FLAG_AVAILABILITY_REAL_TIME_INDICES; }
/** Indicates whether the given unit value is defined */
bool _validSickMeasuringUnits( const sick_lms_2xx_measuring_units_t sick_units ) const;
/** Indicates whether the given scan angle is defined */
bool _validSickScanAngle( const sick_lms_2xx_scan_angle_t sick_scan_angle ) const;
/** Indicates whether the given scan resolution is defined */
bool _validSickScanResolution( const sick_lms_2xx_scan_resolution_t sick_scan_resolution ) const;
/** Indicates whether the given sensitivity is defined */
bool _validSickSensitivity( const sick_lms_2xx_sensitivity_t sick_sensitivity ) const;
/** Indicates whether the given peak threshold is defined */
bool _validSickPeakThreshold( const sick_lms_2xx_peak_threshold_t sick_peak_threshold ) const;
/** Indicates whether the given sensitivity is defined */
bool _validSickMeasuringMode( const sick_lms_2xx_measuring_mode_t sick_measuring_mode ) const;
/** Indicates whether the Sick LMS is type 200 */
bool _isSickLMS200( ) const;
/** Indicates whether the Sick LMS is type 211 */
bool _isSickLMS211( ) const;
/** Indicates whether the Sick LMS is type 220 */
bool _isSickLMS220( ) const;
/** Indicates whether the Sick LMS is type 221 */
bool _isSickLMS221( ) const;
/** Indicates whether the Sick LMS is type 291 */
bool _isSickLMS291( ) const;
/** Indicates whether the Sick LMS type is unknown */
bool _isSickUnknown( ) const;
/** Given a baud rate as an integer, gets a LMS baud rate command. */
sick_lms_2xx_baud_t _baudToSickBaud( const int baud_rate ) const;
/** Given a bytecode representing Sick LMS availability, returns a corresponding string */
std::string _sickAvailabilityToString( const uint8_t availability_code ) const;
/** Given a bytecode representing Sick LMS restart mode, returns a corresponding string */
std::string _sickRestartToString( const uint8_t restart_code ) const;
/** Converts the LMS's temporary field value to a string */
std::string _sickTemporaryFieldToString( const uint8_t temp_field_code ) const;
/** Converts the LMS's subtractive field value to a string */
std::string _sickSubtractiveFieldsToString( const uint8_t subt_field_code ) const;
/** Converts the LMS's contour function status code to a string */
std::string _sickContourFunctionToString( const uint8_t contour_function_code ) const;
/** Converts the LMS's variant to a corresponding string */
std::string _sickVariantToString( const unsigned int sick_variant ) const;
};
/*!
* \typedef sick_lms_2xx_type_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_type_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_type_t sick_lms_2xx_type_t;
/*!
* \typedef sick_lms_2xx_variant_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_variant_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_variant_t sick_lms_2xx_variant_t;
/*!
* \typedef sick_lms_2xx_scan_angle_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_scan_angle_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_scan_angle_t sick_lms_2xx_scan_angle_t;
/*!
* \typedef sick_lms_2xx_scan_resolution_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_scan_resolution_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_scan_resolution_t sick_lms_2xx_scan_resolution_t;
/*!
* \typedef sick_lms_2xx_measuring_units_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_measuring_units_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_measuring_units_t sick_lms_2xx_measuring_units_t;
/*!
* \typedef sick_lms_2xx_sensitivity_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_sensitivity_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_sensitivity_t sick_lms_2xx_sensitivity_t;
/*!
* \typedef sick_lms_2xx_peak_threshold_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_peak_threshold_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_peak_threshold_t sick_lms_2xx_peak_threshold_t;
/*!
* \typedef sick_lms_2xx_status_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_status_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_status_t sick_lms_2xx_status_t;
/*!
* \typedef sick_lms_2xx_measuring_mode_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_measuring_mode_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_measuring_mode_t sick_lms_2xx_measuring_mode_t;
/*!
* \typedef sick_lms_2xx_operating_mode_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_operating_mode_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_operating_mode_t sick_lms_2xx_operating_mode_t;
/*!
* \typedef sick_lms_2xx_baud_t
* \brief Makes working w/ SickLMS2xx::sick_lms_2xx_baud_t a bit easier
*/
typedef SickLMS2xx::sick_lms_2xx_baud_t sick_lms_2xx_baud_t;
} //namespace SickToolbox
#endif //SICK_LMS_2XX_HH

51
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS2xxBufferMonitor.hh Normal file
View File

@ -0,0 +1,51 @@
/*!
* \file SickLMS2xxBufferMonitor.hh
* \brief Defines a class for monitoring the receive
* buffer when interfacing w/ a Sick LMS 2xx
* laser range finder.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_2XX_BUFFER_MONITOR_HH
#define SICK_LMS_2XX_BUFFER_MONITOR_HH
#define DEFAULT_SICK_LMS_2XX_SICK_BYTE_TIMEOUT (35000) ///< Max allowable time between consecutive bytes
/* Definition dependencies */
#include "SickLMS2xxMessage.hh"
#include "SickBufferMonitor.hh"
#include "SickException.hh"
/* Associate the namespace */
namespace SickToolbox {
/*!
* \brief A class for monitoring the receive buffer when interfacing with a Sick LMS LIDAR
*/
class SickLMS2xxBufferMonitor : public SickBufferMonitor< SickLMS2xxBufferMonitor, SickLMS2xxMessage > {
public:
/** A standard constructor */
SickLMS2xxBufferMonitor( );
/** A method for extracting a single message from the stream */
void GetNextMessageFromDataStream( SickLMS2xxMessage &sick_message ) throw( SickIOException );
/** A standard destructor */
~SickLMS2xxBufferMonitor( );
};
} /* namespace SickToolbox */
#endif /* SICK_LMS_2XX_BUFFER_MONITOR_HH */

99
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS2xxMessage.hh Normal file
View File

@ -0,0 +1,99 @@
/*!
* \file SickLMS2xxMessage.hh
* \brief Definition of class SickLMS2xxMessage.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_LMS_2XX_MESSAGE_HH
#define SICK_LMS_2XX_MESSAGE_HH
/* Definition dependencies */
#include <string.h>
#include <netinet/in.h>
#include "SickMessage.hh"
#include "SickException.hh"
#define CRC16_GEN_POL 0x8005 ///< Used to compute CRCs
/** Makes a "short" in little endian */
#define MKSHORT(a,b) ((unsigned short) (a) | ((unsigned short)(b) << 8))
#define SICK_LMS_2XX_MSG_HEADER_LEN (4) ///< Sick LMS message length in bytes
#define SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN (812) ///< Sick LMS max payload length in bytes
#define SICK_LMS_2XX_MSG_TRAILER_LEN (2) ///< Sick LMS message trailer length in bytes
/* Associate the namespace */
namespace SickToolbox {
/**
* \brief A class to represent all messages sent to and from the Sick LMS 2xx
*
* This class helps to construct messages to be sent to the Sick. It also
* provides a container for received messages to be parsed into.
*/
class SickLMS2xxMessage : public SickMessage< SICK_LMS_2XX_MSG_HEADER_LEN, SICK_LMS_2XX_MSG_PAYLOAD_MAX_LEN, SICK_LMS_2XX_MSG_TRAILER_LEN >
{
public:
/** Default constructor. Constructs an empty message (not well-formed!). */
SickLMS2xxMessage( );
/** Constructs a frame by using BuildMessage(). */
SickLMS2xxMessage( const uint8_t dest_address, const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Constructs a frame using ParseMessage(). */
SickLMS2xxMessage( uint8_t * const message_buffer );
/** Constructs a well-formed raw frame from input fields. */
void BuildMessage( uint8_t dest_address, const uint8_t * const payload_buffer,
const unsigned int payload_length );
/** Populates fields from a (well-formed) raw frame. */
void ParseMessage( const uint8_t * const message_buffer );
/** Gets the address of the frame. */
uint8_t GetDestAddress( ) const { return _message_buffer[1]; }
/** Gets the command code associated with the message */
uint8_t GetCommandCode( ) const { return _message_buffer[MESSAGE_HEADER_LENGTH]; }
/** Gets the status byte from an LMS response message (NOTE: only applies to Sick LMS response telegrams!) */
uint8_t GetStatusByte( ) const { return _message_buffer[MESSAGE_HEADER_LENGTH+_payload_length-1]; }
/** Gets the checksum for the message. */
uint16_t GetChecksum( ) const { return _checksum; }
/** Reset the data associated with this message (for initialization purposes) */
void Clear( );
/** A debugging function that prints the contents of the message. */
void Print( ) const;
/** Destructor */
~SickLMS2xxMessage();
protected:
/** The checksum (CRC16) */
uint16_t _checksum;
private:
/** Computes the checksum of the frame. */
uint16_t _computeCRC( uint8_t * data, unsigned int data_length ) const;
};
} /* namespace SickToolbox */
#endif //SICK_LMS_2XX_MESSAGE_HH

121
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickLMS2xxUtility.hh Normal file
View File

@ -0,0 +1,121 @@
/*!
* \file SickLMS2xxUtility.hh
* \brief Defines simple utility functions for working with the
* Sick LMS 2xx laser range finder units.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
/* Auto-generated header */
#include "SickConfig.hh"
/**
* \def REVERSE_BYTE_ORDER_16
* \brief Reverses the byte order of the given 16 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_16( y ) ( ( ( y & 0x00FF ) << 8 ) | ( ( y & 0xFF00 ) >> 8 ) )
/**
* \def REVERSE_BYTE_ORDER_32
* \brief Reverses the byte order of the given 32 bit unsigned integer
*/
#define REVERSE_BYTE_ORDER_32( y ) ( ( ( y & 0x000000FF ) << 24 ) | ( ( y & 0x0000FF00 ) << 8 ) | ( ( y & 0x00FF0000 ) >> 8 ) | ( ( y & 0xFF000000 ) >> 24 ) )
/* Associate the namespace */
namespace SickToolbox {
#ifndef WORDS_BIGENDIAN
/* NOTE: The following functions are necessary since the Sick LD doesn't adopt the
* convention of converting from network byte order.
*/
/**
* \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint16_t host_to_sick_lms_2xx_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts host byte order (little-endian) to Sick LMS byte order (little-endian)
* \param value The 4-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint32_t host_to_sick_lms_2xx_byte_order( uint32_t value ) {
return value;
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint16_t sick_lms_2xx_to_host_byte_order( uint16_t value ) {
return value;
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (little-endian)
* \param value The 4-byte value to convert to little-endian
* \return Value in host byte order (little-endian)
*/
inline uint32_t sick_lms_2xx_to_host_byte_order( uint32_t value ) {
return value;
}
#else // The host has a big-endian architecture
/**
* \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian)
* \param value The 2-byte value to convert to little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint16_t host_to_sick_lms_2xx_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts host byte order (big-endian) to Sick LMS byte order (little-endian)
* \param value The 4-byte value to convertto little-endian
* \return Value in Sick LMS byte order (little-endian)
*/
inline uint32_t host_to_sick_lms_2xx_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian)
* \param value The 2-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint16_t sick_lms_2xx_to_host_byte_order( uint16_t value ) {
return REVERSE_BYTE_ORDER_16(value);
}
/**
* \brief Converts Sick LMS byte order (little-endian) to host byte order (big-endian)
* \param value The 4-byte value to convert to big-endian
* \return Value in host byte order (big-endian)
*/
inline uint32_t sick_lms_2xx_to_host_byte_order( uint32_t value ) {
return REVERSE_BYTE_ORDER_32(value);
}
#endif /* _LITTLE_ENDIAN_HOST */
/*
* NOTE: Other utility functions can be defined here
*/
} //namespace SickToolbox

230
sitl_config/ugv/sicktoolbox/include/sicktoolbox/SickMessage.hh Normal file
View File

@ -0,0 +1,230 @@
/*!
* \file SickMessage.hh
* \brief Defines the abstract parent class for all Sick messages.
*
* Code by Jason C. Derenick and Thomas H. Miller.
* Contact derenick(at)lehigh(dot)edu
*
* The Sick LIDAR Matlab/C++ Toolbox
* Copyright (c) 2008, Jason C. Derenick and Thomas H. Miller
* All rights reserved.
*
* This software is released under a BSD Open-Source License.
* See http://sicktoolbox.sourceforge.net
*/
#ifndef SICK_MESSAGE
#define SICK_MESSAGE
/* Dependencies */
#include <arpa/inet.h>
#include <iomanip>
#include <iostream>
/* Associate the namespace */
namespace SickToolbox {
/**
* \class SickMessage
* \brief Provides an abstract parent for all Sick messages
*/
template < unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
class SickMessage {
public:
/** Some constants to make things more manageable */
static const unsigned int MESSAGE_HEADER_LENGTH = MSG_HEADER_LENGTH;
static const unsigned int MESSAGE_TRAILER_LENGTH = MSG_TRAILER_LENGTH;
static const unsigned int MESSAGE_PAYLOAD_MAX_LENGTH = MSG_PAYLOAD_MAX_LENGTH;
static const unsigned int MESSAGE_MAX_LENGTH = MESSAGE_HEADER_LENGTH + MESSAGE_PAYLOAD_MAX_LENGTH + MESSAGE_TRAILER_LENGTH;
/** A standard constructor */
SickMessage( );
/** Construct a well-formed Sick message */
void BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length );
/** Populates fields given a sequence of bytes representing a raw message */
virtual void ParseMessage( const uint8_t * const message_buffer ) = 0;
/** Returns a copy of the raw message buffer */
void GetMessage( uint8_t * const message_buffer ) const;
/** Resturns the total message length in bytes */
unsigned int GetMessageLength( ) const { return _message_length; }
/** Returns a copy of the raw message payload */
void GetPayload( uint8_t * const payload_buffer ) const;
/** Returns a copy of the payload as a C String */
void GetPayloadAsCStr( char * const payload_str ) const;
/** Returns a subregion of the payload specified by indices */
void GetPayloadSubregion( uint8_t * const payload_sub_buffer, const unsigned int start_idx,
const unsigned int stop_idx ) const;
/** Returns the total payload length in bytes */
unsigned int GetPayloadLength( ) const { return _payload_length; }
/** Indicates whether the message container is populated */
bool IsPopulated( ) const { return _populated; };
/** Clear the contents of the message container/object */
virtual void Clear( );
/** Print the contents of the message */
virtual void Print( ) const;
/** A virtual destructor */
virtual ~SickMessage( );
protected:
/** The length of the message payload in bytes */
unsigned int _payload_length;
/** The length of the message in bytes */
unsigned int _message_length;
/** The message as a raw sequence of bytes */
uint8_t _message_buffer[MESSAGE_MAX_LENGTH];
/** Indicates whether the message container/object is populated */
bool _populated;
};
/**
* \brief A default constructor
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::SickMessage( ) { }
/**
* \brief Constructs a Sick message given the parameter values
* \param *payload_buffer The payload of the message as an array of bytes
* \param payload_length The length of the payload in bytes
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::BuildMessage( const uint8_t * const payload_buffer, const unsigned int payload_length ) {
/* Clear the object */
Clear();
/* Assign the payload and message lengths */
_payload_length = payload_length;
_message_length = MESSAGE_HEADER_LENGTH + MESSAGE_TRAILER_LENGTH + _payload_length;
/* Copy the payload into the message buffer */
memcpy(&_message_buffer[MESSAGE_HEADER_LENGTH],payload_buffer,_payload_length);
/* Mark the object container as being populated */
_populated = true;
}
/**
* \brief Parses a sequence of bytes into a Sick message
* \param *message_buffer A well-formed message to be parsed into the class' fields
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::ParseMessage( const uint8_t * const message_buffer ) {
/* Clear the message container/object */
Clear();
/* Mark the object as populated */
_populated = true;
}
/**
* \brief Get the message as a sequence of well-formed bytes
* \param *message_buffer Destination buffer for message contents
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetMessage( uint8_t * const message_buffer ) const {
memcpy(message_buffer,_message_buffer,_message_length);
}
/**
* \brief Get the payload contents as a sequence of well-formed bytes
* \param *payload_buffer Destination buffer for message payload contents
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayload( uint8_t * const payload_buffer ) const {
memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length);
}
/** Returns a copy of the payload as a C String */
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadAsCStr( char * const payload_buffer ) const {
memcpy(payload_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH],_payload_length);
payload_buffer[_payload_length] = '\0';
}
/**
* \brief Get a specified sub-region of the payload buffer
* \param *payload_sub_buffer Destination buffer for message payload contents
* \param *start_idx The 0-indexed starting location for copying
* \param *stop_idx The 0-indexed stopping location for copying
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::GetPayloadSubregion( uint8_t * const payload_sub_buffer,
const unsigned int start_idx,
const unsigned int stop_idx ) const {
/* Extract the subregion */
memcpy(payload_sub_buffer,&_message_buffer[MESSAGE_HEADER_LENGTH+start_idx],stop_idx+1);
}
/**
* \brief Reset all internal fields and buffers
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Clear( ) {
/* Reset the parent integer variables */
_message_length = _payload_length = 0;
/* Clear the message buffer */
memset(_message_buffer,0,MESSAGE_MAX_LENGTH);
/* Set the flag indicating this message object/container is empty */
_populated = false;
}
/**
* \brief Print data about this object
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
void SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::Print( ) const {
std::cout << "Payload length: " << GetPayloadLength() << std::endl;
std::cout << "Message length: " << GetMessageLength() << std::endl;
std::cout << std::flush;
std::cout << "Message (hex):" << std::endl;
std::cout.setf(std::ios::hex,std::ios::basefield);
for (unsigned int i = 0; i < _message_length; i++) {
std::cout << (int)_message_buffer[i] << " ";
}
std::cout << std::endl << std::flush;
std::cout << "Message (ASCII):" << std::endl;
std::cout.setf(std::ios::dec,std::ios::basefield);
for (unsigned int i = 0; i < _message_length; i++) {
std::cout << _message_buffer[i] << " ";
}
std::cout << std::endl << std::flush;
}
/**
* \brief A destructor
*/
template< unsigned int MSG_HEADER_LENGTH, unsigned int MSG_PAYLOAD_MAX_LENGTH, unsigned int MSG_TRAILER_LENGTH >
SickMessage< MSG_HEADER_LENGTH, MSG_PAYLOAD_MAX_LENGTH, MSG_TRAILER_LENGTH >::~SickMessage() { }
} /* namespace SickToolbox */
#endif /* SICK_MESSAGE */

BIN
sitl_config/ugv/sicktoolbox/manuals/sicktoolbox-RS-422.pdf Normal file
View File

Binary file not shown.

BIN
sitl_config/ugv/sicktoolbox/manuals/sicktoolbox-quickstart.pdf Normal file
View File

Binary file not shown.

27
sitl_config/ugv/sicktoolbox/package.xml Normal file
View File

@ -0,0 +1,27 @@
<?xml version="1.0"?>
<package>
<name>sicktoolbox</name>
<version>1.0.104</version>
<description>SICK Toolbox drivers for SICK laser rangefinders
This package contains the ROS fork of the SICK LIDAR Matlab/C++ Toolbox, available from <a href="http://sicktoolbox.sourceforge.net/">http://sicktoolbox.sourceforge.net/</a>.
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.
</description>
<maintainer email="chadrockey@gmail.com">Chad Rockey</maintainer>
<license>BSD</license>
<url type="website">http://sicktoolbox.sourceforge.net/</url>
<url type="bugtracker">https://github.com/ros-drivers/sicktoolbox/issues</url>
<url type="repository">https://github.com/ros-drivers/sicktoolbox</url>
<author email="derenick@lehigh.edu">Jason Derenick</author>
<author email="thm204@lehigh.edu">Thomas Miller</author>
<buildtool_depend>catkin</buildtool_depend>
<export>
</export>
</package>