$search
00001 00002 // this program is just a little test to make sure the laser is working. 00003 // it's mostly just to familiarize myself with the sicktoolbox library. 00004 // it's heavily lifted from the sicktoolbox lms_simple_app program. 00005 // 00006 // Copyright (C) 2008, Morgan Quigley 00007 // 00008 // I am distributing this code under the BSD license: 00009 // 00010 // Redistribution and use in source and binary forms, with or without 00011 // modification, are permitted provided that the following conditions are met: 00012 // * Redistributions of source code must retain the above copyright notice, 00013 // this list of conditions and the following disclaimer. 00014 // * Redistributions in binary form must reproduce the above copyright 00015 // notice, this list of conditions and the following disclaimer in the 00016 // documentation and/or other materials provided with the distribution. 00017 // * Neither the name of Stanford University nor the names of its 00018 // contributors may be used to endorse or promote products derived from 00019 // this software without specific prior written permission. 00020 // 00021 // THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 00022 // AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 00023 // IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE 00024 // ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE 00025 // LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR 00026 // CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF 00027 // SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS 00028 // INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN 00029 // CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) 00030 // ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 00031 // POSSIBILITY OF SUCH DAMAGE. 00032 00033 #include <csignal> 00034 extern "C" { 00035 // not everyone has <cstdint> 00036 #include <stdint.h> 00037 } 00038 #include <cstdio> 00039 #include <sicklms-1.0/SickLMS.hh> 00040 using namespace SickToolbox; 00041 using namespace std; 00042 00043 bool got_ctrlc = false; 00044 void ctrlc_handler(int) 00045 { 00046 got_ctrlc = true; 00047 } 00048 00049 int main(int argc, char **argv) 00050 { 00051 if (argc != 3) 00052 { 00053 printf("Usage: print_scans DEVICE BAUD_RATE\n"); 00054 return 1; 00055 } 00056 string lms_dev = argv[1]; 00057 SickLMS::sick_lms_baud_t desired_baud = SickLMS::StringToSickBaud(argv[2]); 00058 if (desired_baud == SickLMS::SICK_BAUD_UNKNOWN) 00059 { 00060 printf("bad baud rate. must be one of {9600, 19200, 38400, 500000}\n"); 00061 return 1; 00062 } 00063 signal(SIGINT, ctrlc_handler); 00064 uint32_t values[SickLMS::SICK_MAX_NUM_MEASUREMENTS] = {0}; 00065 uint32_t num_values = 0; 00066 SickLMS sick_lms(lms_dev); 00067 try 00068 { 00069 sick_lms.Initialize(desired_baud); 00070 } 00071 catch (...) 00072 { 00073 printf("initialize failed! are you using the correct device path?\n"); 00074 } 00075 try 00076 { 00077 while (!got_ctrlc) 00078 { 00079 sick_lms.GetSickScan(values, num_values); 00080 // print 12 ranges to the console 00081 int inc = num_values / 11; 00082 printf("%5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d %5d\n", 00083 values[0], values[inc], 00084 values[2*inc], values[3*inc], 00085 values[4*inc], values[5*inc], 00086 values[6*inc], values[7*inc], 00087 values[8*inc], values[9*inc], 00088 values[10*inc], values[num_values-1]); 00089 } 00090 } 00091 catch (...) 00092 { 00093 printf("woah! error!\n"); 00094 } 00095 try 00096 { 00097 sick_lms.Uninitialize(); 00098 } 00099 catch (...) 00100 { 00101 printf("error during uninitialize\n"); 00102 return 1; 00103 } 00104 printf("success.\n"); 00105 return 0; 00106 } 00107