├── CHANGELOG.rst
├── package.xml
├── src
    ├── neato_laser_publisher.cpp
    └── xv11_laser.cpp
├── include
    └── xv_11_laser_driver
    │   └── xv11_laser.h
└── CMakeLists.txt


/CHANGELOG.rst:
--------------------------------------------------------------------------------
 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 2 | Changelog for package xv_11_laser_driver
 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
 4 | 
 5 | 0.3.0 (2017-03-22)
 6 | ------------------
 7 | * update default firmware_version to 2
 8 | * Contributors: Rohan Agrawal
 9 | 
10 | 0.2.2 (2014-09-15)
11 | ------------------
12 | * added install rule for include files
13 | * Contributors: Rohan Agrawal
14 | 
15 | 0.2.1 (2014-09-14)
16 | ------------------
17 | * merged Steve Dillo's include file
18 | * Merge pull request `#1 <https://github.com/rohbotics/xv_11_laser_driver/issues/1>`_ from rohbotics/master
19 |   Merge master and hydro-devel
20 | * Contributors: Rohan Agrawal
21 | 
22 | 0.2.0 (2014-02-22)
23 | ------------------
24 | * Pulled in Steve 'dillo Okay's RPMs topic
25 | * Contributors: Rohan Agrawal
26 | 
27 | 0.1.2 (2013-11-23)
28 | ------------------
29 | * initial commit
30 | * Contributors: Rohan Agrawal
31 | 


--------------------------------------------------------------------------------
/package.xml:
--------------------------------------------------------------------------------
 1 | <?xml version="1.0"?>
 2 | <package>
 3 |   <name>xv_11_laser_driver</name>
 4 |   <version>0.3.0</version>
 5 |   <description>Neato XV-11 Laser Driver. This driver works with the laser when it is removed from the XV-11 Robot as opposed to reading scans from the Neato's USB port.</description>
 6 | 
 7 |   <!-- One maintainer tag required, multiple allowed, one person per tag --> 
 8 |   <!-- Example:  -->
 9 |   <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
10 |   <maintainer email="send2arohan@gmail.com">rohan</maintainer>
11 | 
12 | 
13 |   <!-- One license tag required, multiple allowed, one license per tag -->
14 |   <!-- Commonly used license strings: -->
15 |   <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
16 |   <license>BSD</license>
17 | 
18 | 
19 |   <!-- Url tags are optional, but mutiple are allowed, one per tag -->
20 |   <!-- Optional attribute type can be: website, bugtracker, or repository -->
21 |   <!-- Example: -->
22 |   <url type="website">http://ros.org/wiki/xv_11_laser_driver</url>
23 | 
24 | 
25 |   <!-- Author tags are optional, mutiple are allowed, one per tag -->
26 |   <!-- Authors do not have to be maintianers, but could be -->
27 |   <!-- Example: -->
28 |   <author>Eric Perko</author>
29 |   <author>Chad Rockey</author>
30 |   <author>Rohan Agrawal</author>
31 |   <author>Steve 'dillo Okay</author>
32 | 
33 | 
34 |   <!-- The *_depend tags are used to specify dependencies -->
35 |   <!-- Dependencies can be catkin packages or system dependencies -->
36 |   <!-- Examples: -->
37 |   <!-- Use build_depend for packages you need at compile time: -->
38 |   <!--   <build_depend>message_generation</build_depend> -->
39 |   <!-- Use buildtool_depend for build tool packages: -->
40 |   <!--   <buildtool_depend>catkin</buildtool_depend> -->
41 |   <!-- Use run_depend for packages you need at runtime: -->
42 |   <!--   <run_depend>message_runtime</run_depend> -->
43 |   <!-- Use test_depend for packages you need only for testing: -->
44 |   <!--   <test_depend>gtest</test_depend> -->
45 |   <buildtool_depend>catkin</buildtool_depend>
46 |   <build_depend>sensor_msgs</build_depend>
47 |   <build_depend>roscpp</build_depend>
48 |   <run_depend>boost</run_depend>
49 | 
50 | 
51 |   <!-- The export tag contains other, unspecified, tags -->
52 |   <export>
53 |     <!-- You can specify that this package is a metapackage here: -->
54 |     <!-- <metapackage/> -->
55 | 
56 |     <!-- Other tools can request additional information be placed here -->
57 | 
58 |   </export>
59 | </package>
60 | 


--------------------------------------------------------------------------------
/src/neato_laser_publisher.cpp:
--------------------------------------------------------------------------------
 1 | /*********************************************************************
 2 |  * Software License Agreement (BSD License)
 3 |  *
 4 |  *  Copyright (c) 2011, Eric Perko, Chad Rockey
 5 |  *  All rights reserved.
 6 |  *
 7 |  *  Redistribution and use in source and binary forms, with or without
 8 |  *  modification, are permitted provided that the following conditions
 9 |  *  are met:
10 |  *
11 |  *   * Redistributions of source code must retain the above copyright
12 |  *     notice, this list of conditions and the following disclaimer.
13 |  *   * Redistributions in binary form must reproduce the above
14 |  *     copyright notice, this list of conditions and the following
15 |  *     disclaimer in the documentation and/or other materials provided
16 |  *     with the distribution.
17 |  *   * Neither the name of Case Western Reserve University nor the names of its
18 |  *     contributors may be used to endorse or promote products derived
19 |  *     from this software without specific prior written permission.
20 |  *
21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 |  *  POSSIBILITY OF SUCH DAMAGE.
33 |  *********************************************************************/
34 | 
35 | #include <ros/ros.h>
36 | #include <sensor_msgs/LaserScan.h>
37 | #include <boost/asio.hpp>
38 | #include <xv_11_laser_driver/xv11_laser.h>
39 | #include <std_msgs/UInt16.h>
40 | 
41 | int main(int argc, char **argv)
42 | {
43 |   ros::init(argc, argv, "neato_laser_publisher");
44 |   ros::NodeHandle n;
45 |   ros::NodeHandle priv_nh("~");
46 | 
47 |   std::string port;
48 |   int baud_rate;
49 |   std::string frame_id;
50 |   int firmware_number;
51 |  
52 |   std_msgs::UInt16 rpms; 
53 | 
54 |   priv_nh.param("port", port, std::string("/dev/ttyUSB0"));
55 |   priv_nh.param("baud_rate", baud_rate, 115200);
56 |   priv_nh.param("frame_id", frame_id, std::string("neato_laser"));
57 |   priv_nh.param("firmware_version", firmware_number, 2);
58 | 
59 |   boost::asio::io_service io;
60 | 
61 |   try {
62 |     xv_11_laser_driver::XV11Laser laser(port, baud_rate, firmware_number, io);
63 |     ros::Publisher laser_pub = n.advertise<sensor_msgs::LaserScan>("scan", 1000);
64 |     ros::Publisher motor_pub = n.advertise<std_msgs::UInt16>("rpms",1000);
65 | 
66 |     while (ros::ok()) {
67 |       sensor_msgs::LaserScan::Ptr scan(new sensor_msgs::LaserScan);
68 |       scan->header.frame_id = frame_id;
69 |       scan->header.stamp = ros::Time::now();
70 |       laser.poll(scan);
71 |       rpms.data=laser.rpms;
72 |       laser_pub.publish(scan);
73 |       motor_pub.publish(rpms);
74 | 
75 |     }
76 |     laser.close();
77 |     return 0;
78 |   } catch (boost::system::system_error ex) {
79 |     ROS_ERROR("Error instantiating laser object. Are you sure you have the correct port and baud rate? Error was %s", ex.what());
80 |     return -1;
81 |   }
82 | }
83 | 


--------------------------------------------------------------------------------
/include/xv_11_laser_driver/xv11_laser.h:
--------------------------------------------------------------------------------
 1 | /*********************************************************************
 2 |  * Software License Agreement (BSD License)
 3 |  *
 4 |  *  Copyright (c) 2011, Eric Perko, Chad Rockey
 5 |  *  All rights reserved.
 6 |  *
 7 |  *  Redistribution and use in source and binary forms, with or without
 8 |  *  modification, are permitted provided that the following conditions
 9 |  *  are met:
10 |  *
11 |  *   * Redistributions of source code must retain the above copyright
12 |  *     notice, this list of conditions and the following disclaimer.
13 |  *   * Redistributions in binary form must reproduce the above
14 |  *     copyright notice, this list of conditions and the following
15 |  *     disclaimer in the documentation and/or other materials provided
16 |  *     with the distribution.
17 |  *   * Neither the name of Case Western Reserve University nor the names of its
18 |  *     contributors may be used to endorse or promote products derived
19 |  *     from this software without specific prior written permission.
20 |  *
21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
32 |  *  POSSIBILITY OF SUCH DAMAGE.
33 |  *********************************************************************/
34 | 
35 | #include <sensor_msgs/LaserScan.h>
36 | #include <boost/asio.hpp>
37 | #include <boost/array.hpp>
38 | #include <string>
39 | 
40 | namespace xv_11_laser_driver {
41 |     class XV11Laser {
42 |         public:
43 | 	      uint16_t rpms; ///< @brief RPMS derived from the rpm bytes in an XV11 packet
44 |             /**
45 |               * @brief Constructs a new XV11Laser attached to the given serial port
46 |               * @param port The string for the serial port device to attempt to connect to, e.g. "/dev/ttyUSB0"
47 |               * @param baud_rate The baud rate to open the serial port at.
48 |               * @param io Boost ASIO IO Service to use when creating the serial port object
49 |               */
50 |             XV11Laser(const std::string& port, uint32_t baud_rate, uint32_t firmware, boost::asio::io_service& io);
51 | 
52 |             /**
53 |               * @brief Default destructor
54 |               */
55 |             ~XV11Laser() {};
56 | 
57 |             /**
58 |               * @brief Poll the laser to get a new scan. Blocks until a complete new scan is received or close is called.
59 |               * @param scan LaserScan message pointer to fill in with the scan. The caller is responsible for filling in the ROS timestamp and frame_id
60 |               */
61 |             void poll(sensor_msgs::LaserScan::Ptr scan);
62 | 
63 |             /**
64 |               * @brief Close the driver down and prevent the polling loop from advancing
65 |               */
66 |             void close() { shutting_down_ = true; };
67 | 
68 |         private:
69 |             std::string port_; ///< @brief The serial port the driver is attached to
70 |             uint32_t baud_rate_; ///< @brief The baud rate for the serial connection
71 |             uint32_t firmware_; ///< @brief The firmware version to check.  Currently supports two different versions: 1 and 2.
72 | 
73 |             bool shutting_down_; ///< @brief Flag for whether the driver is supposed to be shutting down or not
74 |             boost::asio::serial_port serial_; ///< @brief Actual serial port object for reading/writing to the XV11 Laser Scanner
75 | 	    uint16_t motor_speed_; ///< @brief current motor speed as reported by the XV11.
76 |     };
77 | };
78 | 


--------------------------------------------------------------------------------
/CMakeLists.txt:
--------------------------------------------------------------------------------
  1 | cmake_minimum_required(VERSION 2.8.3)
  2 | project(xv_11_laser_driver)
  3 | 
  4 | ## Find catkin macros and libraries
  5 | ## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
  6 | ## is used, also find other catkin packages
  7 | find_package(catkin REQUIRED COMPONENTS roscpp sensor_msgs)
  8 | 
  9 | ## System dependencies are found with CMake's conventions
 10 | find_package(Boost REQUIRED COMPONENTS system)
 11 | 
 12 | 
 13 | ## Uncomment this if the package has a setup.py. This macro ensures
 14 | ## modules and global scripts declared therein get installed
 15 | ## See http://ros.org/doc/groovy/api/catkin/html/user_guide/setup_dot_py.html
 16 | # catkin_python_setup()
 17 | 
 18 | #######################################
 19 | ## Declare ROS messages and services ##
 20 | #######################################
 21 | 
 22 | ## Generate messages in the 'msg' folder
 23 | # add_message_files(
 24 | #   FILES
 25 | #   Message1.msg
 26 | #   Message2.msg
 27 | # )
 28 | 
 29 | ## Generate services in the 'srv' folder
 30 | # add_service_files(
 31 | #   FILES
 32 | #   Service1.srv
 33 | #   Service2.srv
 34 | # )
 35 | 
 36 | ## Generate added messages and services with any dependencies listed here
 37 | # generate_messages(
 38 |  #  DEPENDENCIES
 39 |  # sensor_msgs
 40 | # )
 41 | 
 42 | ###################################
 43 | ## catkin specific configuration ##
 44 | ###################################
 45 | ## The catkin_package macro generates cmake config files for your package
 46 | ## Declare things to be passed to dependent projects
 47 | ## LIBRARIES: libraries you create in this project that dependent projects also need
 48 | ## CATKIN_DEPENDS: catkin_packages dependent projects also need
 49 | ## DEPENDS: system dependencies of this project that dependent projects also need
 50 | catkin_package(
 51 |    INCLUDE_DIRS include
 52 | #  LIBRARIES xv_11_laser_driver
 53 | #  CATKIN_DEPENDS roscpp sensor_msgs
 54 | #  DEPENDS system_lib
 55 | )
 56 | 
 57 | ###########
 58 | ## Build ##
 59 | ###########
 60 | 
 61 | ## Specify additional locations of header files
 62 | ## Your package locations should be listed before other locations
 63 | include_directories(include
 64 |   ${catkin_INCLUDE_DIRS}
 65 | )
 66 | 
 67 | ## Declare a cpp library
 68 | # add_library(xv_11_laser_driver
 69 | #   src/${PROJECT_NAME}/xv_11_laser_driver.cpp
 70 | # )
 71 | 
 72 | ## Declare a cpp executable
 73 | # add_executable(xv_11_laser_driver_node src/xv_11_laser_driver_node.cpp)
 74 | add_executable(neato_laser_publisher 
 75 | 	src/neato_laser_publisher.cpp
 76 | 	src/xv11_laser.cpp
 77 | )
 78 | ## Add cmake target dependencies of the executable/library
 79 | ## as an example, message headers may need to be generated before nodes
 80 | add_dependencies(neato_laser_publisher xv_11_laser_driver_generate_messages_cpp)
 81 | 
 82 | ## Specify libraries to link a library or executable target against
 83 | target_link_libraries(neato_laser_publisher
 84 |   ${catkin_LIBRARIES}
 85 | )
 86 | 
 87 | #############
 88 | ## Install ##
 89 | #############
 90 | 
 91 | # all install targets should use catkin DESTINATION variables
 92 | # See http://ros.org/doc/groovy/api/catkin/html/adv_user_guide/variables.html
 93 | 
 94 | ## Mark executable scripts (Python etc.) for installation
 95 | ## in contrast to setup.py, you can choose the destination
 96 | # install(PROGRAMS
 97 | #   scripts/my_python_script
 98 | #   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
 99 | # )
100 | 
101 | ## Mark executables and/or libraries for installation
102 | install(TARGETS neato_laser_publisher
103 |   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
104 |   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
105 |   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
106 | )
107 | 
108 | ## Mark cpp header files for installation
109 | # install(DIRECTORY include/${PROJECT_NAME}/
110 | #   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
111 | #   FILES_MATCHING PATTERN "*.h"
112 | #   PATTERN ".svn" EXCLUDE
113 | # )
114 | 
115 | ## Mark cpp header files for installation
116 | install(DIRECTORY include/${PROJECT_NAME}/
117 |   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
118 |   FILES_MATCHING PATTERN "*.h"
119 |   PATTERN ".svn" EXCLUDE
120 | )
121 | 
122 | ## Mark other files for installation (e.g. launch and bag files, etc.)
123 | # install(FILES
124 | #   # myfile1
125 | #   # myfile2
126 | #   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
127 | # )
128 | 
129 | #############
130 | ## Testing ##
131 | #############
132 | 
133 | ## Add gtest based cpp test target and link libraries
134 | # catkin_add_gtest(${PROJECT_NAME}-test test/test_xv_11_laser_driver.cpp)
135 | # if(TARGET ${PROJECT_NAME}-test)
136 | #   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
137 | # endif()
138 | 
139 | ## Add folders to be run by python nosetests
140 | # catkin_add_nosetests(test)
141 | 
142 | 


--------------------------------------------------------------------------------
/src/xv11_laser.cpp:
--------------------------------------------------------------------------------
  1 | /*********************************************************************
  2 |  * Software License Agreement (BSD License)
  3 |  *
  4 |  *  Copyright (c) 2011, Eric Perko, Chad Rockey
  5 |  *  All rights reserved.
  6 |  *
  7 |  *  Redistribution and use in source and binary forms, with or without
  8 |  *  modification, are permitted provided that the following conditions
  9 |  *  are met:
 10 |  *
 11 |  *   * Redistributions of source code must retain the above copyright
 12 |  *     notice, this list of conditions and the following disclaimer.
 13 |  *   * Redistributions in binary form must reproduce the above
 14 |  *     copyright notice, this list of conditions and the following
 15 |  *     disclaimer in the documentation and/or other materials provided
 16 |  *     with the distribution.
 17 |  *   * Neither the name of Case Western Reserve University nor the names of its
 18 |  *     contributors may be used to endorse or promote products derived
 19 |  *     from this software without specific prior written permission.
 20 |  *
 21 |  *  THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
 22 |  *  "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
 23 |  *  LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
 24 |  *  FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
 25 |  *  COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
 26 |  *  INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
 27 |  *  BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 28 |  *  LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
 29 |  *  CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
 30 |  *  LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
 31 |  *  ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
 32 |  *  POSSIBILITY OF SUCH DAMAGE.
 33 |  *********************************************************************/
 34 | 
 35 | #include <xv_11_laser_driver/xv11_laser.h>
 36 | 
 37 | namespace xv_11_laser_driver {
 38 |   XV11Laser::XV11Laser(const std::string& port, uint32_t baud_rate, uint32_t firmware, boost::asio::io_service& io): port_(port),
 39 |   baud_rate_(baud_rate), firmware_(firmware), shutting_down_(false), serial_(io, port_) {
 40 |     serial_.set_option(boost::asio::serial_port_base::baud_rate(baud_rate_));
 41 |   }
 42 | 
 43 |   void XV11Laser::poll(sensor_msgs::LaserScan::Ptr scan) {
 44 |     uint8_t temp_char;
 45 |     uint8_t start_count = 0;
 46 |     bool got_scan = false;
 47 |     
 48 |     if(firmware_ == 1){ // This is for the old driver, the one that only outputs speed once per revolution
 49 |       boost::array<uint8_t, 1440> raw_bytes;
 50 |       while (!shutting_down_ && !got_scan) {
 51 | 	// Wait until the start sequence 0x5A, 0xA5, 0x00, 0xC0 comes around
 52 | 	boost::asio::read(serial_, boost::asio::buffer(&temp_char,1));
 53 | 	if(start_count == 0) {
 54 | 	  if(temp_char == 0x5A) {
 55 | 	    start_count = 1;
 56 | 	  }
 57 | 	} else if(start_count == 1) {
 58 | 	  if(temp_char == 0xA5) {
 59 | 	    start_count = 2;
 60 | 	  }
 61 | 	} else if(start_count == 2) {
 62 | 	  if(temp_char == 0x00) {
 63 | 	    start_count = 3;
 64 | 	  }
 65 | 	} else if(start_count == 3) {
 66 | 	  if(temp_char == 0xC0) {
 67 | 	    start_count = 0;
 68 | 	    // Now that entire start sequence has been found, read in the rest of the message
 69 | 	    got_scan = true;
 70 | 	    // Now read speed
 71 | 	    boost::asio::read(serial_,boost::asio::buffer(&motor_speed_,2));
 72 | 
 73 | 	    // Read in 360*4 = 1440 chars for each point
 74 | 	    boost::asio::read(serial_,boost::asio::buffer(&raw_bytes,1440));
 75 | 
 76 | 	    scan->angle_min = 0.0;
 77 | 	    scan->angle_max = 2.0*M_PI;
 78 | 	    scan->angle_increment = (2.0*M_PI/360.0);
 79 | 	    scan->time_increment = motor_speed_/1e8;
 80 | 	    scan->range_min = 0.06;
 81 | 	    scan->range_max = 5.0;
 82 | 	    scan->ranges.reserve(360);
 83 | 	    scan->intensities.reserve(360);
 84 | 
 85 | 	    for(uint16_t i = 0; i < raw_bytes.size(); i=i+4) {
 86 | 	      // Four bytes per reading
 87 | 	      uint8_t byte0 = raw_bytes[i];
 88 | 	      uint8_t byte1 = raw_bytes[i+1];
 89 | 	      uint8_t byte2 = raw_bytes[i+2];
 90 | 	      uint8_t byte3 = raw_bytes[i+3];
 91 | 	      // First two bits of byte1 are status flags
 92 | 	      uint8_t flag1 = (byte1 & 0x80) >> 7;  // No return/max range/too low of reflectivity
 93 | 	      uint8_t flag2 = (byte1 & 0x40) >> 6;  // Object too close, possible poor reading due to proximity kicks in at < 0.6m
 94 | 	      // Remaining bits are the range in mm
 95 | 	      uint16_t range = ((byte1 & 0x3F)<< 8) + byte0;
 96 | 	      // Last two bytes represent the uncertanty or intensity, might also be pixel area of target...
 97 | 	      uint16_t intensity = (byte3 << 8) + byte2;
 98 | 
 99 | 	      scan->ranges.push_back(range / 1000.0);
100 | 	      scan->intensities.push_back(intensity);
101 | 	    }
102 | 	  }
103 | 	}
104 |       }
105 |     } else if(firmware_ == 2) { // This is for the newer driver that outputs packets 4 pings at a time
106 |       boost::array<uint8_t, 1980> raw_bytes;
107 |       uint8_t good_sets = 0;
108 |       uint32_t motor_speed = 0;
109 |       rpms=0;
110 |       int index;
111 |       while (!shutting_down_ && !got_scan) {
112 | 	// Wait until first data sync of frame: 0xFA, 0xA0
113 | 	boost::asio::read(serial_, boost::asio::buffer(&raw_bytes[start_count],1));
114 | 	if(start_count == 0) {
115 | 	  if(raw_bytes[start_count] == 0xFA) {
116 | 	    start_count = 1;
117 | 	  }
118 | 	} else if(start_count == 1) {
119 | 	  if(raw_bytes[start_count] == 0xA0) {
120 | 	    start_count = 0;
121 | 
122 | 	    // Now that entire start sequence has been found, read in the rest of the message
123 | 	    got_scan = true;
124 | 
125 | 	    boost::asio::read(serial_,boost::asio::buffer(&raw_bytes[2], 1978));
126 | 
127 | 	    scan->angle_min = 0.0;
128 | 	    scan->angle_max = 2.0*M_PI;
129 | 	    scan->angle_increment = (2.0*M_PI/360.0);
130 | 	    scan->range_min = 0.06;
131 | 	    scan->range_max = 5.0;
132 | 	    scan->ranges.resize(360);
133 | 	    scan->intensities.resize(360);
134 | 
135 | 	    //read data in sets of 4
136 | 	    for(uint16_t i = 0; i < raw_bytes.size(); i=i+22) {
137 | 	      if(raw_bytes[i] == 0xFA && raw_bytes[i+1] == (0xA0+i/22)) {//&& CRC check
138 | 		good_sets++;
139 | 		motor_speed += (raw_bytes[i+3] << 8) + raw_bytes[i+2]; //accumulate count for avg. time increment
140 |        		rpms=(raw_bytes[i+3]<<8|raw_bytes[i+2])/64; 
141 | 		
142 | 		for(uint16_t j = i+4; j < i+20; j=j+4) {
143 | 		  index = (4*i)/22 + (j-4-i)/4;
144 | 		  // Four bytes per reading
145 | 		  uint8_t byte0 = raw_bytes[j];
146 | 		  uint8_t byte1 = raw_bytes[j+1];
147 | 		  uint8_t byte2 = raw_bytes[j+2];
148 | 		  uint8_t byte3 = raw_bytes[j+3];
149 | 		  // First two bits of byte1 are status flags
150 | 		  // uint8_t flag1 = (byte1 & 0x80) >> 7;  // No return/max range/too low of reflectivity
151 | 		  // uint8_t flag2 = (byte1 & 0x40) >> 6;  // Object too close, possible poor reading due to proximity kicks in at < 0.6m
152 | 		  // Remaining bits are the range in mm
153 | 		  uint16_t range = ((byte1 & 0x3F)<< 8) + byte0;
154 | 		  // Last two bytes represent the uncertanty or intensity, might also be pixel area of target...
155 | 		  uint16_t intensity = (byte3 << 8) + byte2;
156 | 
157 | 		  scan->ranges[index] = range / 1000.0;
158 | 		  scan->intensities[index] = intensity;
159 | 		}
160 | 	      }
161 | 	    }
162 | 
163 | 	    scan->time_increment = motor_speed/good_sets/1e8;
164 | 	  }
165 | 	}
166 |       }
167 |     }
168 |   }
169 | };
170 | 


--------------------------------------------------------------------------------