xv11_laser.cpp
Go to the documentation of this file.
00001 /*********************************************************************
00002  * Software License Agreement (BSD License)
00003  *
00004  *  Copyright (c) 2011, Eric Perko, Chad Rockey
00005  *  All rights reserved.
00006  *
00007  *  Redistribution and use in source and binary forms, with or without
00008  *  modification, are permitted provided that the following conditions
00009  *  are met:
00010  *
00011  *   * Redistributions of source code must retain the above copyright
00012  *     notice, this list of conditions and the following disclaimer.
00013  *   * Redistributions in binary form must reproduce the above
00014  *     copyright notice, this list of conditions and the following
00015  *     disclaimer in the documentation and/or other materials provided
00016  *     with the distribution.
00017  *   * Neither the name of Case Western Reserve University nor the names of its
00018  *     contributors may be used to endorse or promote products derived
00019  *     from this software without specific prior written permission.
00020  *
00021  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
00022  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
00023  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
00024  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
00025  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
00026  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
00027  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00028  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
00029  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
00030  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
00031  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
00032  *  POSSIBILITY OF SUCH DAMAGE.
00033  *********************************************************************/
00034 
00035 #include <xv_11_laser_driver/xv11_laser.h>
00036 
00037 namespace xv_11_laser_driver {
00038   XV11Laser::XV11Laser(const std::string& port, uint32_t baud_rate, uint32_t firmware, boost::asio::io_service& io): port_(port),
00039   baud_rate_(baud_rate), firmware_(firmware), shutting_down_(false), serial_(io, port_) {
00040     serial_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
00041   }
00042 
00043   void XV11Laser::poll(sensor_msgs::LaserScan::Ptr scan) {
00044     uint8_t temp_char;
00045     uint8_t start_count = 0;
00046     bool got_scan = false;
00047     
00048     if(firmware_ == 1){ // This is for the old driver, the one that only outputs speed once per revolution
00049       boost::array<uint8_t, 1440> raw_bytes;
00050       while (!shutting_down_ && !got_scan) {
00051         // Wait until the start sequence 0x5A, 0xA5, 0x00, 0xC0 comes around
00052         boost::asio::read(serial_, boost::asio::buffer(&temp_char,1));
00053         if(start_count == 0) {
00054           if(temp_char == 0x5A) {
00055             start_count = 1;
00056           }
00057         } else if(start_count == 1) {
00058           if(temp_char == 0xA5) {
00059             start_count = 2;
00060           }
00061         } else if(start_count == 2) {
00062           if(temp_char == 0x00) {
00063             start_count = 3;
00064           }
00065         } else if(start_count == 3) {
00066           if(temp_char == 0xC0) {
00067             start_count = 0;
00068             // Now that entire start sequence has been found, read in the rest of the message
00069             got_scan = true;
00070             // Now read speed
00071             boost::asio::read(serial_,boost::asio::buffer(&motor_speed_,2));
00072 
00073             // Read in 360*4 = 1440 chars for each point
00074             boost::asio::read(serial_,boost::asio::buffer(&raw_bytes,1440));
00075 
00076             scan->angle_min = 0.0;
00077             scan->angle_max = 2.0*M_PI;
00078             scan->angle_increment = (2.0*M_PI/360.0);
00079             scan->time_increment = motor_speed_/1e8;
00080             scan->range_min = 0.06;
00081             scan->range_max = 5.0;
00082             scan->ranges.reserve(360);
00083             scan->intensities.reserve(360);
00084 
00085             for(uint16_t i = 0; i < raw_bytes.size(); i=i+4) {
00086               // Four bytes per reading
00087               uint8_t byte0 = raw_bytes[i];
00088               uint8_t byte1 = raw_bytes[i+1];
00089               uint8_t byte2 = raw_bytes[i+2];
00090               uint8_t byte3 = raw_bytes[i+3];
00091               // First two bits of byte1 are status flags
00092               uint8_t flag1 = (byte1 & 0x80) >> 7;  // No return/max range/too low of reflectivity
00093               uint8_t flag2 = (byte1 & 0x40) >> 6;  // Object too close, possible poor reading due to proximity kicks in at < 0.6m
00094               // Remaining bits are the range in mm
00095               uint16_t range = ((byte1 & 0x3F)<< 8) + byte0;
00096               // Last two bytes represent the uncertanty or intensity, might also be pixel area of target...
00097               uint16_t intensity = (byte3 << 8) + byte2;
00098 
00099               scan->ranges.push_back(range / 1000.0);
00100               scan->intensities.push_back(intensity);
00101             }
00102           }
00103         }
00104       }
00105     } else if(firmware_ == 2) { // This is for the newer driver that outputs packets 4 pings at a time
00106       boost::array<uint8_t, 1980> raw_bytes;
00107       uint8_t good_sets = 0;
00108       uint32_t motor_speed = 0;
00109       rpms=0;
00110       int index;
00111       while (!shutting_down_ && !got_scan) {
00112         // Wait until first data sync of frame: 0xFA, 0xA0
00113         boost::asio::read(serial_, boost::asio::buffer(&raw_bytes[start_count],1));
00114         if(start_count == 0) {
00115           if(raw_bytes[start_count] == 0xFA) {
00116             start_count = 1;
00117           }
00118         } else if(start_count == 1) {
00119           if(raw_bytes[start_count] == 0xA0) {
00120             start_count = 0;
00121 
00122             // Now that entire start sequence has been found, read in the rest of the message
00123             got_scan = true;
00124 
00125             boost::asio::read(serial_,boost::asio::buffer(&raw_bytes[2], 1978));
00126 
00127             scan->angle_min = 0.0;
00128             scan->angle_max = 2.0*M_PI;
00129             scan->angle_increment = (2.0*M_PI/360.0);
00130             scan->range_min = 0.06;
00131             scan->range_max = 5.0;
00132             scan->ranges.resize(360);
00133             scan->intensities.resize(360);
00134 
00135             //read data in sets of 4
00136             for(uint16_t i = 0; i < raw_bytes.size(); i=i+22) {
00137               if(raw_bytes[i] == 0xFA && raw_bytes[i+1] == (0xA0+i/22)) {//&& CRC check
00138                 good_sets++;
00139                 motor_speed += (raw_bytes[i+3] << 8) + raw_bytes[i+2]; //accumulate count for avg. time increment
00140                 rpms=(raw_bytes[i+3]<<8|raw_bytes[i+2])/64; 
00141                 
00142                 for(uint16_t j = i+4; j < i+20; j=j+4) {
00143                   index = (4*i)/22 + (j-4-i)/4;
00144                   // Four bytes per reading
00145                   uint8_t byte0 = raw_bytes[j];
00146                   uint8_t byte1 = raw_bytes[j+1];
00147                   uint8_t byte2 = raw_bytes[j+2];
00148                   uint8_t byte3 = raw_bytes[j+3];
00149                   // First two bits of byte1 are status flags
00150                   // uint8_t flag1 = (byte1 & 0x80) >> 7;  // No return/max range/too low of reflectivity
00151                   // uint8_t flag2 = (byte1 & 0x40) >> 6;  // Object too close, possible poor reading due to proximity kicks in at < 0.6m
00152                   // Remaining bits are the range in mm
00153                   uint16_t range = ((byte1 & 0x3F)<< 8) + byte0;
00154                   // Last two bytes represent the uncertanty or intensity, might also be pixel area of target...
00155                   uint16_t intensity = (byte3 << 8) + byte2;
00156 
00157                   scan->ranges[index] = range / 1000.0;
00158                   scan->intensities[index] = intensity;
00159                 }
00160               }
00161             }
00162 
00163             scan->time_increment = motor_speed/good_sets/1e8;
00164           }
00165         }
00166       }
00167     }
00168   }
00169 };


xv_11_laser_driver
Author(s): Eric Perko, Chad Rockey, Rohan Agrawal, Steve 'dillo Okay
autogenerated on Fri Aug 28 2015 13:41:38