├── test ├── TODO └── unplug_test.launch ├── src ├── CMakeLists.txt ├── getID.cpp ├── getFirmwareVersion.cpp ├── timestamp_test.cpp ├── hokuyo_node.cpp └── hokuyo.cpp ├── hokuyo_test.launch ├── hokuyo_test_intensity.launch ├── package.xml ├── hokuyo_test.vcg ├── CMakeLists.txt ├── cfg └── Hokuyo.cfg ├── CHANGELOG.rst ├── include └── hokuyo_node │ └── hokuyo.h └── COPYING.lib /test/TODO: -------------------------------------------------------------------------------- 1 | Need to check: 2 | * Unplug/replug doesn't kill the driver. 3 | * Starting up with the laser scanning doesn't cause problems. 4 | -------------------------------------------------------------------------------- /test/unplug_test.launch: -------------------------------------------------------------------------------- 1 | 2 | 4 | 5 | 6 | 7 | 8 | -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | find_library(LOG4CXX_LIBRARY log4cxx) 2 | if(NOT LOG4CXX_LIBRARY) 3 | message(FATAL_ERROR "Couldn't find log4cxx library") 4 | endif() 5 | 6 | rosbuild_add_executable(getID getID.cpp) 7 | target_link_libraries(getID libhokuyo ${LOG4CXX_LIBRARY}) 8 | 9 | rosbuild_add_executable(getFirmwareVersion getFirmwareVersion.cpp) 10 | target_link_libraries(getFirmwareVersion libhokuyo ${LOG4CXX_LIBRARY}) 11 | -------------------------------------------------------------------------------- /hokuyo_test.launch: -------------------------------------------------------------------------------- 1 | 15 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 31 | 32 | 33 | -------------------------------------------------------------------------------- /hokuyo_test_intensity.launch: -------------------------------------------------------------------------------- 1 | 16 | 17 | 18 | 19 | 20 | 21 | 22 | 23 | 24 | 25 | 26 | 27 | 28 | 29 | 32 | 33 | 34 | -------------------------------------------------------------------------------- /package.xml: -------------------------------------------------------------------------------- 1 | 2 | hokuyo_node 3 | 1.7.8 4 | A ROS node to provide access to SCIP 2.0-compliant Hokuyo laser range finders (including 04LX). 5 | Chad Rockey 6 | 7 | LGPL 8 | 9 | http://www.ros.org/wiki/hokuyo_node 10 | https://github.com/ros-drivers/hokuyo_node/issues 11 | https://github.com/ros-drivers/hokuyo_node 12 | 13 | Brian P. Gerkey 14 | Jeremy Leibs 15 | Blaise Gassend 16 | 17 | catkin 18 | 19 | rosconsole 20 | roscpp 21 | sensor_msgs 22 | driver_base 23 | self_test 24 | diagnostic_updater 25 | dynamic_reconfigure 26 | log4cxx 27 | 28 | rosconsole 29 | roscpp 30 | sensor_msgs 31 | driver_base 32 | self_test 33 | diagnostic_updater 34 | dynamic_reconfigure 35 | log4cxx 36 | 37 | 38 | -------------------------------------------------------------------------------- /hokuyo_test.vcg: -------------------------------------------------------------------------------- 1 | Background\ ColorR=0 2 | Background\ ColorG=0 3 | Background\ ColorB=0 4 | Fixed\ Frame=/laser 5 | Target\ Frame=/laser 6 | Floor\ Scan.Alpha=1 7 | Floor\ Scan.Autocompute\ Intensity\ Bounds=1 8 | Floor\ Scan.Billboard\ Size=0.01 9 | Floor\ Scan.Channel=-1 10 | Floor\ Scan.Decay\ Time=0 11 | Floor\ Scan.Enabled=1 12 | Floor\ Scan.Max\ ColorR=0 13 | Floor\ Scan.Max\ ColorG=1 14 | Floor\ Scan.Max\ ColorB=0 15 | Floor\ Scan.Max\ Intensity=4096 16 | Floor\ Scan.Min\ ColorR=0 17 | Floor\ Scan.Min\ ColorG=0 18 | Floor\ Scan.Min\ ColorB=0 19 | Floor\ Scan.Min\ Intensity=0 20 | Floor\ Scan.Selectable=1 21 | Floor\ Scan.Style=1 22 | Floor\ Scan.Topic=/scan 23 | Grid.Alpha=0.5 24 | Grid.Cell\ Size=1 25 | Grid.ColorR=0.5 26 | Grid.ColorG=0.5 27 | Grid.ColorB=0.5 28 | Grid.Enabled=1 29 | Grid.Line\ Style=0 30 | Grid.Line\ Width=0.03 31 | Grid.Normal\ Cell\ Count=0 32 | Grid.OffsetX=0 33 | Grid.OffsetY=0 34 | Grid.OffsetZ=0 35 | Grid.Plane=0 36 | Grid.Plane\ Cell\ Count=10 37 | Grid.Reference\ Frame= 38 | Tool\ 2D\ Nav\ GoalTopic=goal 39 | Tool\ 2D\ Pose\ EstimateTopic=initialpose 40 | Camera\ Type=rviz::FixedOrientationOrthoViewController 41 | Camera\ Config=121.003 -0.957785 11.3941 -0.649553 -0.707107 -0 -0 0.707107 42 | Property\ Grid\ State=selection=;expanded=.Global Options;scrollpos=0,0;splitterpos=125,266;ispageselected=1 43 | [Display0] 44 | Name=Floor Scan 45 | Package=rviz 46 | ClassName=rviz::LaserScanDisplay 47 | [Display1] 48 | Name=Grid 49 | Package=rviz 50 | ClassName=rviz::GridDisplay 51 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 2.8.3) 2 | project(hokuyo_node) 3 | 4 | find_package(Boost REQUIRED COMPONENTS system thread) 5 | 6 | # Utilities that use log4cxx - getID and getFirmwareVersion 7 | find_library(LOG4CXX_LIBRARY log4cxx) 8 | if(NOT LOG4CXX_LIBRARY) 9 | message(FATAL_ERROR "Couldn't find log4cxx library") 10 | endif() 11 | 12 | # Load catkin and all dependencies required for this package 13 | find_package(catkin REQUIRED 14 | COMPONENTS 15 | diagnostic_updater 16 | driver_base 17 | dynamic_reconfigure 18 | rosconsole 19 | roscpp 20 | self_test 21 | sensor_msgs 22 | ) 23 | 24 | include_directories(SYSTEM ${Boost_INCLUDE_DIRS}) 25 | include_directories(include ${catkin_INCLUDE_DIRS}) 26 | 27 | # Dynamic reconfigure support 28 | generate_dynamic_reconfigure_options(cfg/Hokuyo.cfg) 29 | 30 | # Export with catkin_package 31 | catkin_package( 32 | INCLUDE_DIRS include 33 | CATKIN_DEPENDS rosconsole 34 | LIBRARIES libhokuyo 35 | ) 36 | 37 | # Hokuyo library 38 | add_library(libhokuyo src/hokuyo.cpp) 39 | target_link_libraries(libhokuyo ${rosconsole_LIBRARIES}) 40 | 41 | # hokuyo_node 42 | add_executable(hokuyo_node src/hokuyo_node.cpp) 43 | add_dependencies(hokuyo_node ${PROJECT_NAME}_gencfg) 44 | target_link_libraries(hokuyo_node 45 | libhokuyo 46 | ${Boost_LIBRARIES} 47 | ${catkin_LIBRARIES} 48 | ) 49 | 50 | add_executable(getID src/getID.cpp) 51 | target_link_libraries(getID libhokuyo ${LOG4CXX_LIBRARY} ${catkin_LIBRARIES}) 52 | 53 | add_executable(getFirmwareVersion src/getFirmwareVersion.cpp) 54 | target_link_libraries(getFirmwareVersion 55 | libhokuyo 56 | ${LOG4CXX_LIBRARY} 57 | ${catkin_LIBRARIES} 58 | ) 59 | 60 | # Install targets 61 | install(TARGETS libhokuyo DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}) 62 | 63 | install(DIRECTORY include/${PROJECT_NAME}/ 64 | DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} 65 | ) 66 | 67 | install(TARGETS hokuyo_node getID getFirmwareVersion 68 | DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} 69 | ) 70 | 71 | install(FILES test/TODO test/unplug_test.launch COPYING.lib hokuyo_test.launch hokuyo_test.vcg hokuyo_test_intensity.launch 72 | DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} 73 | ) -------------------------------------------------------------------------------- /src/getID.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 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 the Willow Garage 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 "hokuyo_node/hokuyo.h" 36 | #include 37 | #include 38 | #include 39 | 40 | using namespace std; 41 | 42 | 43 | int 44 | main(int argc, char** argv) 45 | { 46 | if (argc < 2 || argc > 3) 47 | { 48 | fprintf(stderr, "usage: getID /dev/ttyACM? [quiet]\nOutputs the device ID of a hokuyo at /dev/ttyACM?. Add a second argument for script friendly output.\n"); 49 | return 1; 50 | } 51 | 52 | bool verbose = (argc == 2); 53 | 54 | if (!verbose) 55 | { 56 | // In quiet mode we want to turn off logging levels that go to stdout. 57 | log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME); 58 | logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]); 59 | ros::console::notifyLoggerLevelsChanged(); 60 | } 61 | 62 | hokuyo::Laser laser; 63 | 64 | for (int retries = 10; retries; retries--) 65 | { 66 | try 67 | { 68 | laser.open(argv[1]); 69 | std::string device_id = laser.getID(); 70 | if (verbose) 71 | printf("Device at %s has ID ", argv[1]); 72 | printf("%s\n", device_id.c_str()); 73 | laser.close(); 74 | return 0; 75 | } 76 | catch (hokuyo::Exception &e) 77 | { 78 | if (verbose) 79 | ROS_WARN("getID failed: %s", e.what()); 80 | laser.close(); 81 | } 82 | sleep(1); 83 | } 84 | 85 | if (verbose) 86 | ROS_ERROR("getID failed for 10 seconds. Giving up."); 87 | return 1; 88 | } 89 | -------------------------------------------------------------------------------- /src/getFirmwareVersion.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2009, Willow Garage, Inc. 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 the Willow Garage 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 "hokuyo_node/hokuyo.h" 36 | #include 37 | #include 38 | #include 39 | 40 | using namespace std; 41 | 42 | 43 | int 44 | main(int argc, char** argv) 45 | { 46 | if (argc < 2 || argc > 3) 47 | { 48 | fprintf(stderr, "usage: getFirmwareVersion /dev/ttyACM? [quiet]\nOutputs the firmware version of a hokuyo at /dev/ttyACM?. Add a second argument for script friendly output.\n"); 49 | return 1; 50 | } 51 | 52 | bool verbose = (argc == 2); 53 | 54 | if (!verbose) 55 | { 56 | // In quiet mode we want to turn off logging levels that go to stdout. 57 | log4cxx::LoggerPtr logger = log4cxx::Logger::getLogger(ROSCONSOLE_DEFAULT_NAME); 58 | logger->setLevel(ros::console::g_level_lookup[ros::console::levels::Error]); 59 | ros::console::notifyLoggerLevelsChanged(); 60 | } 61 | 62 | hokuyo::Laser laser; 63 | 64 | for (int retries = 10; retries; retries--) 65 | { 66 | try 67 | { 68 | laser.open(argv[1]); 69 | std::string firmware_version = laser.getFirmwareVersion(); 70 | if (verbose) 71 | printf("Device at %s has FW rev ", argv[1]); 72 | printf("%s\n", firmware_version.c_str()); 73 | laser.close(); 74 | return 0; 75 | } 76 | catch (hokuyo::Exception &e) 77 | { 78 | if (verbose) 79 | ROS_WARN("getFirmwareVersion failed: %s", e.what()); 80 | laser.close(); 81 | } 82 | sleep(1); 83 | } 84 | 85 | if (verbose) 86 | ROS_ERROR("getFirmwareVersion failed for 10 seconds. Giving up."); 87 | return 1; 88 | } 89 | -------------------------------------------------------------------------------- /cfg/Hokuyo.cfg: -------------------------------------------------------------------------------- 1 | #! /usr/bin/env python 2 | #* All rights reserved. 3 | #* 4 | #* Redistribution and use in source and binary forms, with or without 5 | #* modification, are permitted provided that the following conditions 6 | #* are met: 7 | #* 8 | #* * Redistributions of source code must retain the above copyright 9 | #* notice, this list of conditions and the following disclaimer. 10 | #* * Redistributions in binary form must reproduce the above 11 | #* copyright notice, this list of conditions and the following 12 | #* disclaimer in the documentation and/or other materials provided 13 | #* with the distribution. 14 | #* * Neither the name of the Willow Garage nor the names of its 15 | #* contributors may be used to endorse or promote products derived 16 | #* from this software without specific prior written permission. 17 | #* 18 | #* THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 19 | #* "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 20 | #* LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 21 | #* FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 22 | #* COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 23 | #* INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 24 | #* BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 25 | #* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 26 | #* CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 27 | #* LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 28 | #* ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 29 | #* POSSIBILITY OF SUCH DAMAGE. 30 | #*********************************************************** 31 | 32 | # Author: Blaise Gassend 33 | 34 | PACKAGE='hokuyo_node' 35 | from dynamic_reconfigure.parameter_generator_catkin import * 36 | 37 | from math import pi 38 | 39 | from driver_base.msg import SensorLevels 40 | 41 | gen = ParameterGenerator() 42 | # Name Type Reconfiguration level Description Default Min Max 43 | gen.add("min_ang", double_t, SensorLevels.RECONFIGURE_STOP, "The angle of the first range measurement. The unit depends on ~ang_radians.", -pi/2, -pi, pi) 44 | gen.add("max_ang", double_t, SensorLevels.RECONFIGURE_STOP, "The angle of the first range measurement. The unit depends on ~ang_radians.", pi/2, -pi, pi) 45 | gen.add("intensity", bool_t, SensorLevels.RECONFIGURE_STOP, "Whether or not the hokuyo returns intensity values.", False) 46 | gen.add("cluster", int_t, SensorLevels.RECONFIGURE_STOP, "The number of adjacent range measurements to cluster into a single reading", 1, 0, 99) 47 | gen.add("skip", int_t, SensorLevels.RECONFIGURE_STOP, "The number of scans to skip between each measured scan", 0, 0, 9) 48 | gen.add("port", str_t, SensorLevels.RECONFIGURE_CLOSE, "The serial port where the hokuyo device can be found", "/dev/ttyACM0") 49 | gen.add("calibrate_time", bool_t, SensorLevels.RECONFIGURE_CLOSE, "Whether the node should calibrate the hokuyo's time offset", True) 50 | gen.add("frame_id", str_t, SensorLevels.RECONFIGURE_RUNNING, "The frame in which laser scans will be returned", "laser") 51 | gen.add("time_offset", double_t, SensorLevels.RECONFIGURE_RUNNING, "An offet to add to the timestamp before publication of a scan", 0, -0.25, 0.25) 52 | gen.add("allow_unsafe_settings",bool_t, SensorLevels.RECONFIGURE_CLOSE, "Turn this on if you wish to use the UTM-30LX with an unsafe angular range. Turning this option on may cause occasional crashes or bad data. This option is a tempory workaround that will hopefully be removed in an upcoming driver version.", False) 53 | 54 | exit(gen.generate(PACKAGE, "hokuyo_node", "Hokuyo")) -------------------------------------------------------------------------------- /src/timestamp_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | static uint64_t timeHelper() 7 | { 8 | #if POSIX_TIMERS > 0 9 | struct timespec curtime; 10 | clock_gettime(CLOCK_REALTIME, &curtime); 11 | return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec); 12 | #else 13 | struct timeval timeofday; 14 | gettimeofday(&timeofday,NULL); 15 | return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000; 16 | #endif 17 | } 18 | 19 | template 20 | C median(std::vector &v) 21 | { 22 | std::vector::iterator start = v.begin(); 23 | std::vector::iterator end = v.end(); 24 | std::vector::iterator median = start + (end - start) / 2; 25 | //std::vector::iterator quarter1 = median - (end - start) / 4; 26 | //std::vector::iterator quarter2 = median + (end - start) / 4; 27 | std::nth_element(start, median, end); 28 | //long long int medianval = *median; 29 | //std::nth_element(start, quarter1, end); 30 | //long long int quarter1val = *quarter1; 31 | //std::nth_element(start, quarter2, end); 32 | //long long int quarter2val = *quarter2; 33 | return *median; 34 | } 35 | 36 | long long int getClockOffset(hokuyo::Laser &laser, int reps) 37 | { 38 | std::vector offset(reps); 39 | int timeout = 1000; 40 | 41 | laser.sendCmd("TM0",timeout); 42 | for (int i = 0; i < reps; i++) 43 | { 44 | long long int prestamp = timeHelper(); 45 | laser.sendCmd("TM1",timeout); 46 | long long int hokuyostamp = laser.readTime(); 47 | long long int poststamp = timeHelper(); 48 | offset[i] = hokuyostamp - (prestamp + poststamp) / 2; 49 | printf("%lli %lli %lli - ", prestamp, hokuyostamp, poststamp); 50 | } 51 | laser.sendCmd("TM2",timeout); 52 | 53 | return median(offset); 54 | } 55 | 56 | long long int getScanOffset(hokuyo::Laser &laser, int reps) 57 | { 58 | std::vector offset(reps); 59 | int timeout = 1000; 60 | 61 | if (reps > 99) 62 | reps = 99; 63 | 64 | if (laser.requestScans(1, -2, 2, 0, 0, reps, timeout) != 0) 65 | { 66 | fprintf(stderr, "Error requesting scan.\n"); 67 | return 1; 68 | } 69 | 70 | hokuyo::LaserScan scan; 71 | for (int i = 0; i < reps; i++) 72 | { 73 | laser.serviceScan(scan, timeout); 74 | offset[i] = scan.self_time_stamp - scan.system_time_stamp; 75 | printf("%lli %lli - ", scan.self_time_stamp, scan.system_time_stamp); 76 | } 77 | 78 | return median(offset); 79 | } 80 | 81 | long long int getDataOffset(hokuyo::Laser &laser, int cycles) 82 | { 83 | int ckreps = 1; 84 | int scanreps = 1; 85 | long long int pre = getClockOffset(laser, ckreps); 86 | std::vector samples(cycles); 87 | for (int i = 0; i < cycles; i++) 88 | { 89 | long long int scan = getScanOffset(laser, ckreps); 90 | long long int post = getClockOffset(laser, scanreps); 91 | samples[i] = scan - (post+pre)/2; 92 | // printf("%lli %lli %lli %lli\n", samples[i], post, pre, scan); 93 | // fflush(stdout); 94 | pre = post; 95 | } 96 | 97 | printf("%lli\n", median(samples)); 98 | return median(samples); 99 | } 100 | 101 | int main(int argc, char **argv) 102 | { 103 | if (argc != 2) 104 | { 105 | fprintf(stderr, "usage: timestamp_test \n"); 106 | return 1; 107 | } 108 | 109 | char *port = argv[1]; 110 | 111 | hokuyo::Laser laser; 112 | 113 | //printf("# %s\n", laser.getFirmwareVersion().c_str()); 114 | //fprintf(stderr, "%s\n", laser.getFirmwareVersion().c_str()); 115 | 116 | /* 117 | int timeout = 1000; 118 | laser.sendCmd("TM0",timeout); 119 | long long int pc_start = timeHelper(); 120 | laser.sendCmd("TM1",timeout); 121 | long long int hokuyo_start = laser.readTime(); 122 | for (int i = 0; i < 10000; i++) 123 | { 124 | laser.sendCmd("TM1",timeout); 125 | long long int hokuyo_time = laser.readTime() - hokuyo_start; 126 | long long int pc_time = timeHelper() - pc_start; 127 | printf("%lli %lli\n", hokuyo_time, pc_time); 128 | fflush(stdout); 129 | } 130 | */ 131 | /*int timeout = 1000; 132 | for (int i = 0; i < 10; i++) 133 | { 134 | fprintf(stderr, "."); 135 | laser.sendCmd("TM0",timeout); 136 | laser.sendCmd("TM1",timeout); 137 | long long int hokuyo_time = laser.readTime(); 138 | long long int pc_time = timeHelper(); 139 | laser.sendCmd("TM2",timeout); 140 | 141 | if (laser.requestScans(0, -1, 1, 0, 0, 1, timeout) != 0) 142 | { 143 | fprintf(stderr, "Error requesting scan.\n"); 144 | return 1; 145 | } 146 | 147 | hokuyo::LaserScan scan; 148 | laser.serviceScan(scan, timeout); 149 | long long int hokuyo_time2 = scan.self_time_stamp; 150 | long long int pc_time2 = scan.system_time_stamp; 151 | 152 | printf("%lli %lli %lli %lli\n", hokuyo_time, pc_time, hokuyo_time2, pc_time2); 153 | fflush(stdout); 154 | }*/ 155 | 156 | //long long int initial_offset = getClockOffset(laser, 20); 157 | //long long int end_offset = getClockOffset(laser, 20); 158 | 159 | /*while (true) 160 | { 161 | getDataOffset(laser, 1); 162 | fflush(stdout); 163 | } */ 164 | 165 | hokuyo::LaserScan scan_; 166 | 167 | laser.open(port); 168 | while (true) 169 | { 170 | try { 171 | //std::string status = laser.getStatus(); 172 | //if (status == std::string("Sensor works well.")) 173 | { 174 | laser.laserOn(); 175 | laser.requestScans(true, -2.3562, 2.3562, 0, 0); 176 | //laser.serviceScan(scan_); 177 | laser.serviceScan(scan_); 178 | } 179 | //else 180 | // fprintf(stderr, "%s\n", laser.getStatus().c_str()); 181 | } 182 | catch (hokuyo::Exception e) 183 | { 184 | fprintf(stderr, "%s\n", e.what()); 185 | } 186 | try { 187 | laser.stopScanning(); 188 | } 189 | catch (hokuyo::Exception e) 190 | { 191 | fprintf(stderr, "%s\n", e.what()); 192 | } 193 | //usleep(100000); 194 | try { 195 | //laser.close(); 196 | } 197 | catch (hokuyo::Exception e) 198 | { 199 | fprintf(stderr, "%s\n", e.what()); 200 | } 201 | } 202 | } 203 | -------------------------------------------------------------------------------- /CHANGELOG.rst: -------------------------------------------------------------------------------- 1 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 2 | Changelog for package hokuyo_node 3 | ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ 4 | 5 | 1.7.8 (2014-01-15) 6 | ------------------ 7 | * Merge pull request `#8 `_ from trainman419/hydro-devel 8 | Use rosconsole to set logger levels. Fixes `#7 `_ 9 | * Use rosconsole to set logger levels. Fixes `#7 `_ 10 | * Added changelog 11 | * Contributors: Chad Rockey, chadrockey, trainman419 12 | 13 | 1.7.7 (2013-07-17) 14 | ------------------ 15 | * fixing value interpretation for smaller lasers (04LX) 16 | 17 | 1.7.6 (2013-04-25) 18 | ------------------ 19 | * Removed REP 117 Tick/Tock for Hydro. 20 | 21 | 1.7.5 (2013-03-26) 22 | ------------------ 23 | * Added install targets for various files. 24 | * Added install targets. 25 | * Fixed two compile errors on OS X with clang 26 | * Fixed dynamic_reconfigure build bug and refactor 27 | * Missing include file. 28 | * Catkinizing 29 | * Tock on REP 117. Will now default to True. Warns if not set or false. 30 | * Had to actually add boost directors and link to boost::thread in the CMakeLists 31 | * Removed call to directory with no CMakeLists.txt 32 | * explicitly link log4cxx 33 | * Updated hokuyo node to add the use_rep_117 parameter and output error cases accordingly. 34 | * Added a launch file to help test whether hokuyo unplug causes SIGHUP. 35 | * Now only polls when there is no buffered data. Otherwise could lock up. Now only checks timeout once in read loop. Now detects EOF. Verified that no sighup on unplug. Now just need to test on Mac. Should fix `#3826 `_. 36 | * Tweaked the tty settings to try to get hokuyo_node working on Mac. Seems to be working. Now need to test that I haven't broken anything. 37 | * Now IO to hokuyo is purely file descriptor based. Should help with `#3826 `_. 38 | * Changed open with fopen in the hokuyo library. This allow the NO_CTTY and O_NONBLOCK flags to be set at open time, which simplifies the code a bit. Still need to verify that CTTY actually works. 39 | * Added Ubuntu platform tags to manifest 40 | * Fixed linker export in manifest. 41 | * Now blocks SIGHUP if there isn't a handler for it. 42 | * Updated warning message when angular range is being limited for safety to give more data on why the problem is happening. 43 | * Increased the max_safe_angular_range for 1.16.01 firmware. 44 | * Correcting console output in getFirmwareVersion 45 | * Added error code to diagnostic summary message as requested by `#3705 `_. 46 | * Added getFirmwareVersion tool. 47 | * Improved angle limiting on 30LX so that any sufficiently limited angular rance will work. Also reduced the range to 95 degrees/cluster, because that's what it takes to get good data. 48 | * Made the allow_unsafe_settings warning message appear at more appropriate times. 49 | * Added an allow_unsafe_settings option to bypass the UTM-30LX angular range limits. 50 | * Added detection of 30LX, and automatic angle limiting in that case to prevent laser crashes. 51 | * Added optional logging of hokuyo transactions. Have a small test that crashes the laser. 52 | * Had add algorithm include in wrong place. 53 | * Added missing include of algorithm 54 | * Replaced the time calibration method. The new one is much faster, and much less sensitive to the massive clock error on the new hokuyo firmware. Added a test program that can be used for exploring hokuyo timing issues. 55 | * Added code to detect the new hokuyo firmware version, and use a hard-coded latency for it. Now calls calibration code each time the device_id changes, instead of only once at startup. 56 | * Made some more methods public so that other programs can do relatively low-level access. 57 | * Added queryVersionInformation method to allow the version information to be explicitely reread at a time later than the open call. 58 | * Fixed typo in previous commit. 59 | * Made hokuyo export libhokuyo. Enhances external API (but it is an experimental API). 60 | * Added ability to clear the latency set by calcLatency. Added currently used latency and time offset to diagnostics. 61 | * Added time_offset parameter to allow tweaking of time offset. 62 | * Took out compile twice hack 63 | * preparing laser_drivers 1.0.0. Updated manifest descriptions slightly 64 | * Changed diagnostic level messages. 65 | * Previous fix to clear_window was broken. Moved to postOpenHook instead. 66 | * Moved clear_window for frequency diagnostics to doStart so that the diagnostics get reset on any start event. (Was only in reconfigure events.) 67 | * Tweaked a message. (Calibration will take up to a minute.) 68 | * Cleaned up messages. Now says when it has started streaming. Made reads non-blocking to avoid lockup if concurrent access to the port happens. Added workaround for limited precision with which parameters are stored on the parameter server. Merged Connection Status diagnostic into Driver Status diagnostic. 69 | * Added TODO list of tests 70 | * Made turning off laser more robust. Previously there were errors if the turn off command happened just as a scan was arriving. 71 | * Rearranged stop message to be after the scanning actually stops. 72 | * Now only checks angle ranges and intensity support during config_update in the open state. Checking those in the running state was wreaking havoc, as it should. 73 | * Got rid of all output to stderr when getID is in quiet mode. 74 | * Made the serial number scheme compatible with how it used to be (always H in front), but compatible with long serial numbers (don't start with H when returned by device). 75 | * Made getID retry a few times as the hokuyo takes a while before it can be talked to after it is plugged in. 76 | * Made exceptions during startup TM2 command be ignored. Indeed, if the laser is scanning then TM2 fails, but the subsequent command successfully resets the state. Made the subsequent RS instead of QT as it does a more thorough reset. 77 | * Increased limit on hokuyo exception messages. 78 | * Added detection of intensity capability. Node now automatically sets intensity to false if it is unsupported. 79 | * hokuyo_node now reports firmware version, vendor name, product name and firmware version in diagnostics. (Requested by `#3505 `_) 80 | * Added graceful handling of out-of-range scan range. 81 | * Made hokuyo range go from -pi to pi again because different hokuyos have different ranges, and we can't hard-code some particular range. 82 | * Took race condition out of port locking, and removed unnecessary unlock before close. 83 | * Tweaked package description. Added a debug message. 84 | * Updated licenses. 85 | * Fixed up example launch files. 86 | * Reworked messages so that the driver should never spew when it is failing to reconnect. 87 | * Removed no longer necessary tf stuff from hokuyo example launch file. 88 | * Removed one unnecessary startup message. 89 | * Took node documentation out of doxygen. 90 | * staging laser_drivers into tick-tock 91 | -------------------------------------------------------------------------------- /include/hokuyo_node/hokuyo.h: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2008-2010 Willow Garage 4 | * 5 | * This library is free software; you can redistribute it and/or 6 | * modify it under the terms of the GNU Lesser General Public 7 | * License as published by the Free Software Foundation; either 8 | * version 2.1 of the License, or (at your option) any later version. 9 | * 10 | * This library is distributed in the hope that it will be useful, 11 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 12 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 13 | * Lesser General Public License for more details. 14 | * 15 | * You should have received a copy of the GNU Lesser General Public 16 | * License along with this library; if not, write to the Free Software 17 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 18 | * 19 | */ 20 | 21 | /* \mainpage hokuyo_node 22 | * \htmlinclude manifest.html 23 | */ 24 | 25 | #ifndef HOKUYO_HH 26 | #define HOKUYO_HH 27 | 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | 35 | //! A namespace containing the hokuyo device driver 36 | namespace hokuyo 37 | { 38 | //! The maximum number of readings that can be returned from a hokuyo 39 | const uint32_t MAX_READINGS = 1128; 40 | 41 | //! The maximum length a command will ever be 42 | const int MAX_CMD_LEN = 100; 43 | 44 | //! The maximum number of bytes that should be skipped when looking for a response 45 | const int MAX_SKIPPED = 1000000; 46 | 47 | //! Macro for defining an exception with a given parent (std::runtime_error should be top parent) 48 | #define DEF_EXCEPTION(name, parent) \ 49 | class name : public parent { \ 50 | public: \ 51 | name(const char* msg) : parent(msg) {} \ 52 | }\ 53 | 54 | //! A standard Hokuyo exception 55 | DEF_EXCEPTION(Exception, std::runtime_error); 56 | 57 | //! An exception for use when a timeout is exceeded 58 | DEF_EXCEPTION(TimeoutException, Exception); 59 | 60 | //! An exception for use when data is corrupted 61 | DEF_EXCEPTION(CorruptedDataException, Exception); 62 | 63 | //! An exception for use when the timestamp on the data is repeating (a good indicator something has gone wrong) 64 | DEF_EXCEPTION(RepeatedTimeException, Exception); 65 | 66 | #undef DEF_EXCEPTION 67 | 68 | //! A struct for returning configuration from the Hokuyo 69 | struct LaserConfig 70 | { 71 | //! Start angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. 72 | float min_angle; 73 | //! Stop angle for the laser scan [rad]. 0 is forward and angles are measured clockwise when viewing hokuyo from the top. 74 | float max_angle; 75 | //! Scan resolution [rad]. 76 | float ang_increment; 77 | //! Scan resoltuion [s] 78 | float time_increment; 79 | //! Time between scans 80 | float scan_time; 81 | //! Minimum range [m] 82 | float min_range; 83 | //! Maximum range [m] 84 | float max_range; 85 | //! Range Resolution [m] 86 | float range_res; 87 | }; 88 | 89 | 90 | //! A struct for returning laser readings from the Hokuyo 91 | struct LaserScan 92 | { 93 | //! Array of ranges 94 | std::vector ranges; 95 | //! Array of intensities 96 | std::vector intensities; 97 | //! Self reported time stamp in nanoseconds 98 | uint64_t self_time_stamp; 99 | //! System time when first range was measured in nanoseconds 100 | uint64_t system_time_stamp; 101 | //! Configuration of scan 102 | LaserConfig config; 103 | }; 104 | 105 | 106 | //! A class for interfacing to an SCIP2.0-compliant Hokuyo laser device 107 | /*! 108 | * Almost all methods of this class may throw an exception of type 109 | * hokuyo::Exception in the event of an error when communicating 110 | * with the device. This primarily include read/write problems 111 | * (hokuyo::Exception), communication time-outs 112 | * (hokuyo:TimeoutException), and corrupted data 113 | * (hokuyo::CorruptedDataException). However, many methods which 114 | * wrap commands that are sent to the hokuyo will return a 115 | * hokuyo-supplied status code as well. These are not 116 | * "exceptional," in that the device is operating correctly from a 117 | * communication standpoint, although the return code may still 118 | * indicate that the device has undergone some sort of failure. In 119 | * these cases, return codes of 0 are normal operation. For more 120 | * information on the possible device error codes, consult the 121 | * hokuyo manual. 122 | */ 123 | class Laser 124 | { 125 | public: 126 | //! Constructor 127 | Laser(); 128 | 129 | //! Destructor 130 | ~Laser(); 131 | 132 | //! Open the port 133 | /*! 134 | * This must be done before the hokuyo can be used. This call essentially 135 | * wraps fopen, with some additional calls to tcsetattr. 136 | * 137 | * \param port_name A character array containing the name of the port 138 | * 139 | */ 140 | void open(const char * port_name); 141 | 142 | //! Close the port 143 | /*! 144 | * This call essentiall wraps fclose. 145 | */ 146 | void close(); 147 | 148 | //! Reset the laser to its power on state. 149 | /*! 150 | * This calls TM2 and RS. Exceptions raised by TM2 are ignored as they 151 | * are normal in the case where the laser is scanning. Exceptions 152 | * raised by RS are passed back to the caller. 153 | */ 154 | void reset(); 155 | 156 | //! Check whether the port is open 157 | bool portOpen() { return laser_fd_ != -1; } 158 | 159 | //! Sends an SCIP2.0 command to the hokuyo device 160 | // sets up model 04LX to work in SCIP 2.0 mode 161 | void setToSCIP2(); 162 | 163 | /*! 164 | * This function allows commands to be sent directly to the hokuyo. 165 | * Possible commands can be found in the hokuyo documentation. It 166 | * will only read up until the end of the status code. Any 167 | * additional bytes will be left on the line for parsing. 168 | * 169 | * \param cmd Command and arguments in a character array 170 | * \param timeout Timeout in milliseconds. 171 | * 172 | * \return Status code returned from hokuyo device. 173 | */ 174 | int sendCmd(const char* cmd, int timeout = -1); 175 | 176 | //! Retrieve the native hokuyo configuration. 177 | /*! 178 | * This function will retrieve the native configuration for the laser. 179 | * In particular, this specifies the smallest possible min, and largest 180 | * possible maximum values that can be used. 181 | * 182 | * \param config A reference to the LaserConfig which will be populated 183 | * 184 | * \return Status code returned from hokuyo device. 185 | */ 186 | void getConfig(LaserConfig& config); 187 | 188 | 189 | //! Retrieve a single scan from the hokuyo 190 | /*! 191 | * \param scan A reference to the LaserScan which will be populated 192 | * \param min_ang Minimal angle of the scan 193 | * \param max_ang Maximum angle of the scan 194 | * \param clustering Number of adjascent ranges to be clustered into a single measurement. 195 | * \param timeout Timeout in milliseconds. 196 | * 197 | * \return Status code returned from hokuyo device. 198 | */ 199 | int pollScan(LaserScan& scan, double min_ang, double max_ang, int clustering = 0, int timeout = -1); 200 | 201 | //! Request a sequence of scans from the hokuyo 202 | /*! 203 | * This function will request a sequence of scans from the hokuyo. If 204 | * indefinite scans are requested, stop_scanning() must be called 205 | * to terminate scanning. Service_scan() must be called repeatedly 206 | * to receive scans as they come in. 207 | * 208 | * \param intensity Whether or not intensity data should be provided 209 | * \param min_ang Minimal angle of the scan (radians) 210 | * \param max_ang Maximum angle of the scan (radians) 211 | * \param clustering Number of adjascent ranges to be clustered into a single measurement 212 | * \param skip Number of scans to skip between returning measurements 213 | * \param num Number of scans to return (0 for indefinite) 214 | * \param timeout Timeout in milliseconds. 215 | * 216 | * \return Status code returned from hokuyo device. 217 | */ 218 | int requestScans(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); 219 | 220 | //! Retrieve a scan if they have been requested 221 | /*! 222 | * \param scan A reference to the LaserScan which will be populated. 223 | * \param timeout Timeout in milliseconds. 224 | * 225 | * \return 0 on succes, Status code returned from hokuyo device on failure. (Normally 99 is used for success) 226 | */ 227 | int serviceScan(LaserScan& scan, int timeout = -1); 228 | 229 | //! Turn the laser off 230 | /*! 231 | * \return Status code returned from hokuyo device. 232 | */ 233 | int laserOff(); 234 | 235 | //! Turn the laser on 236 | /*! 237 | * \return Status code returned from hokuyo device. 238 | */ 239 | int laserOn(); 240 | 241 | //! Stop retrieving scans 242 | /*! 243 | * \return Status code returned from hokuyo device. 244 | */ 245 | int stopScanning(); 246 | 247 | //! Return hokuyo serial number 248 | /*! 249 | * \return Serial number of hokuyo. 250 | */ 251 | std::string getID(); 252 | 253 | //! Get status message from the hokuyo 254 | /*! 255 | * \return Status message 256 | */ 257 | std::string getStatus(); 258 | 259 | //! Compute latency between actual hokuyo readings and system time 260 | /*! 261 | * This function will estimate the latency between when the data 262 | * is actually acquired by the hokuyo and a read call returns from 263 | * the hokuyo with the data. This latency is stored in an offset 264 | * variable and applied to the measurement of system time which 265 | * occurs immediately after a read. This means that the 266 | * system_time_stamp variable on LaserScan struct is corrected 267 | * and accurately reflects the exact time of the hokuyo data in 268 | * computer system time. This function takes the same arguments 269 | * as a call to request scans, since this latency may be 270 | * parameter dependent. NOTE: This method will take 271 | * approximately 10 seconds to return. 272 | * 273 | * \param intensity Whether or not intensity data should be provided 274 | * \param min_ang Minimal angle of the scan (radians) 275 | * \param max_ang Maximum angle of the scan (radians) 276 | * \param clustering Number of adjascent ranges to be clustered into a single measurement 277 | * \param skip Number of scans to skip between returning measurements 278 | * \param num Number of times to repeat the measurement 0 for auto. 279 | * \param timeout Timeout in milliseconds. 280 | * 281 | * \return Median latency in nanoseconds 282 | */ 283 | long long calcLatency(bool intensity, double min_ang, double max_ang, int clustering = 0, int skip = 0, int num = 0, int timeout = -1); 284 | 285 | //! This method clears the offset that was computed with calcLatency. 286 | void clearLatency() 287 | { 288 | offset_ = 0; 289 | } 290 | 291 | //! Returns the latency that is in use. 292 | //! \return Latency in nanoseconds. 293 | long long getLatency() 294 | { 295 | return offset_; 296 | } 297 | 298 | //! Return firmware version 299 | /*! 300 | * \return Firmware version of hokuyo. 301 | */ 302 | std::string getFirmwareVersion(); 303 | 304 | //! Return hokuyo vendor name 305 | /*! 306 | * \return Vendor name of hokuyo. 307 | */ 308 | std::string getVendorName(); 309 | 310 | //! Return hokuyo product name 311 | /*! 312 | * \return Product name of hokuyo. 313 | */ 314 | std::string getProductName(); 315 | 316 | //! Return hokuyo protocol version 317 | /*! 318 | * \return Protocol version of hokuyo. 319 | */ 320 | std::string getProtocolVersion(); 321 | 322 | //! Tries to determine if intensity mode is supported 323 | /*! 324 | * \return True if intensity mode is supported, false if it is not. 325 | * hokuyo::Exception if no scan could be read even out of intensity 326 | * mode. 327 | */ 328 | bool isIntensitySupported(); 329 | 330 | //! Call VV and store the returned version information 331 | //! Use other methods to read the information. 332 | void queryVersionInformation(); 333 | 334 | //! Wrapper around fputs 335 | int laserWrite(const char* msg); 336 | 337 | //! Read a full line from the hokuyo using fgets 338 | int laserReadline(char *buf, int len, int timeout = -1); 339 | 340 | //! Wrapper around tcflush 341 | int laserFlush(); 342 | 343 | //! Read in a time value 344 | uint64_t readTime(int timeout = -1); 345 | 346 | private: 347 | //! Use the TM command to determine the offset between the PC clock and 348 | //the Hokuyo clock. Returns the median of reps measurements. 349 | long long int getHokuyoClockOffset(int reps, int timeout = -1); 350 | 351 | //! Get reps scans and returns the median difference between the system 352 | // timestamp (PC) and the scan timestamp (Hokuyo). 353 | long long int getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout = -1); 354 | 355 | //! Query the sensor configuration of the hokuyo 356 | void querySensorConfig(); 357 | 358 | //! Search for a particular sequence and then read the rest of the line 359 | char* laserReadlineAfter(char *buf, int len, const char *str, int timeout = -1); 360 | 361 | //! Compute the checksum of a given buffer 362 | bool checkSum(const char* buf, int buf_len); 363 | 364 | //! Read in a scan 365 | void readData(LaserScan& scan, bool has_intensity, int timout = -1); 366 | 367 | int dmin_; 368 | int dmax_; 369 | int ares_; 370 | int amin_; 371 | int amax_; 372 | int afrt_; 373 | int rate_; 374 | 375 | int wrapped_; 376 | 377 | unsigned int last_time_; 378 | 379 | unsigned int time_repeat_count_; 380 | 381 | long long offset_; 382 | 383 | int laser_fd_; 384 | 385 | std::string vendor_name_; 386 | std::string product_name_; 387 | std::string serial_number_; 388 | std::string protocol_version_; 389 | std::string firmware_version_; 390 | 391 | char read_buf[256]; 392 | int read_buf_start; 393 | int read_buf_end; 394 | }; 395 | 396 | } 397 | 398 | #endif 399 | -------------------------------------------------------------------------------- /src/hokuyo_node.cpp: -------------------------------------------------------------------------------- 1 | /********************************************************************* 2 | * Software License Agreement (BSD License) 3 | * 4 | * Copyright (c) 2008-2010, Willow Garage, Inc. 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 the Willow Garage 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 "driver_base/driver.h" 36 | #include "driver_base/driver_node.h" 37 | #include 38 | 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | 45 | #include "ros/ros.h" 46 | 47 | #include "sensor_msgs/LaserScan.h" 48 | 49 | #include "hokuyo_node/HokuyoConfig.h" 50 | 51 | #include "hokuyo_node/hokuyo.h" 52 | 53 | using namespace std; 54 | 55 | class HokuyoDriver : public driver_base::Driver 56 | { 57 | friend class HokuyoNode; 58 | 59 | typedef boost::function UseScanFunction; 60 | UseScanFunction useScan_; 61 | 62 | boost::shared_ptr scan_thread_; 63 | 64 | std::string device_status_; 65 | std::string device_id_; 66 | std::string last_seen_device_id_; 67 | 68 | bool first_scan_; 69 | 70 | std::string vendor_name_; 71 | std::string product_name_; 72 | std::string protocol_version_; 73 | std::string firmware_version_; 74 | 75 | std::string connect_fail_; 76 | 77 | hokuyo::LaserScan scan_; 78 | hokuyo::Laser laser_; 79 | hokuyo::LaserConfig laser_config_; 80 | 81 | bool calibrated_; 82 | int lost_scan_thread_count_; 83 | int corrupted_scan_count_; 84 | 85 | public: 86 | hokuyo_node::HokuyoConfig config_; 87 | typedef hokuyo_node::HokuyoConfig Config; 88 | 89 | HokuyoDriver() 90 | { 91 | calibrated_ = false; 92 | lost_scan_thread_count_ = 0; 93 | corrupted_scan_count_ = 0; 94 | } 95 | 96 | bool checkAngleRange(Config &conf) 97 | { 98 | bool changed = false; 99 | 100 | if (conf.min_ang < laser_config_.min_angle) 101 | { 102 | changed = true; 103 | if (laser_config_.min_angle - conf.min_ang > 1e-10) /// @todo Avoids warning when restarting node pending ros#2353 getting fixed. 104 | { 105 | ROS_WARN("Requested angle (%f rad) out of range, using minimum scan angle supported by device: %f rad.", 106 | conf.min_ang, laser_config_.min_angle); 107 | } 108 | conf.min_ang = laser_config_.min_angle; 109 | } 110 | 111 | double max_safe_angular_range_per_cluster_deg = 95; 112 | if (firmware_version_ == "1.16.01(16/Nov./2009)") 113 | max_safe_angular_range_per_cluster_deg = 190; 114 | 115 | int real_cluster = conf.cluster == 0 ? 1 : conf.cluster; 116 | double max_safe_angular_range = (real_cluster * max_safe_angular_range_per_cluster_deg) * M_PI / 180; 117 | 118 | if (conf.intensity && (conf.max_ang - conf.min_ang) > max_safe_angular_range + 1e-8 && 119 | !config_.allow_unsafe_settings && laser_.getProductName() == 120 | "SOKUIKI Sensor TOP-URG UTM-30LX") 121 | { 122 | changed = true; 123 | conf.max_ang = conf.min_ang + max_safe_angular_range; 124 | ROS_WARN("More than %f degree/cluster scan range requested on UTM-30LX firmware version %s in intensity mode with cluster=%i. The max_ang was adjusted to limit the range. You may extend the scanner's angular range using the allow_unsafe_settings option, but this may result in incorrect data or laser crashes that will require a power cycle of the laser.", max_safe_angular_range_per_cluster_deg, firmware_version_.c_str(), real_cluster); 125 | } 126 | 127 | if (conf.max_ang - laser_config_.max_angle > 1e-10) /// @todo Avoids warning when restarting node pending ros#2353 getting fixed. 128 | { 129 | changed = true; 130 | ROS_WARN("Requested angle (%f rad) out of range, using maximum scan angle supported by device: %f rad.", 131 | conf.max_ang, laser_config_.max_angle); 132 | conf.max_ang = laser_config_.max_angle; 133 | } 134 | 135 | if (conf.min_ang > conf.max_ang) 136 | { 137 | changed = true; 138 | if (conf.max_ang < laser_config_.min_angle) 139 | { 140 | if (laser_config_.min_angle - conf.max_ang > 1e-10) /// @todo Avoids warning when restarting node pending ros#2353 getting fixed. 141 | ROS_WARN("Requested angle (%f rad) out of range, using minimum scan angle supported by device: %f rad.", 142 | conf.max_ang, laser_config_.min_angle); 143 | conf.max_ang = laser_config_.min_angle; 144 | } 145 | ROS_WARN("Minimum angle must be greater than maximum angle. Adjusting min_ang."); 146 | conf.min_ang = conf.max_ang; 147 | } 148 | 149 | return changed; 150 | } 151 | 152 | bool checkIntensitySupport(Config &conf) 153 | { 154 | if (conf.intensity && !laser_.isIntensitySupported()) 155 | { 156 | ROS_WARN("This unit does not appear to support intensity mode. Turning intensity off."); 157 | conf.intensity = false; 158 | return true; 159 | } 160 | return false; 161 | } 162 | 163 | void doOpen() 164 | { 165 | try 166 | { 167 | std::string old_device_id = device_id_; 168 | device_id_ = "unknown"; 169 | device_status_ = "unknown"; 170 | first_scan_ = true; 171 | 172 | laser_.open(config_.port.c_str()); 173 | 174 | device_id_ = getID(); 175 | vendor_name_ = laser_.getVendorName(); 176 | firmware_version_ = laser_.getFirmwareVersion(); 177 | product_name_ = laser_.getProductName(); 178 | protocol_version_ = laser_.getProtocolVersion(); 179 | 180 | device_status_ = laser_.getStatus(); 181 | if (device_status_ != std::string("Sensor works well.")) 182 | { 183 | doClose(); 184 | setStatusMessagef("Laser returned abnormal status message, aborting: %s You may be able to find further information at http://www.ros.org/wiki/hokuyo_node/Troubleshooting/", device_status_.c_str()); 185 | return; 186 | } 187 | 188 | if (old_device_id != device_id_) 189 | { 190 | ROS_INFO("Connected to device with ID: %s", device_id_.c_str()); 191 | 192 | if (last_seen_device_id_ != device_id_) 193 | { 194 | // Recalibrate when the device changes. 195 | last_seen_device_id_ = device_id_; 196 | calibrated_ = false; 197 | } 198 | 199 | // Do this elaborate retry circuis if we were just plugged in. 200 | for (int retries = 10;; retries--) 201 | try { 202 | laser_.laserOn(); 203 | break; 204 | } 205 | catch (hokuyo::Exception &e) 206 | { 207 | if (!retries) 208 | throw e; // After trying for 10 seconds, give up and throw the exception. 209 | else if (retries == 10) 210 | ROS_WARN("Could not turn on laser. This may happen just after the device is plugged in. Will retry for 10 seconds."); 211 | ros::Duration(1).sleep(); 212 | } 213 | } 214 | else 215 | laser_.laserOn(); // Otherwise, it should just work, so no tolerance. 216 | 217 | if (config_.calibrate_time && !calibrated_) 218 | { 219 | ROS_INFO("Starting calibration. This will take up a few seconds."); 220 | double latency = laser_.calcLatency(false && config_.intensity, config_.min_ang, config_.max_ang, config_.cluster, config_.skip) * 1e-9; 221 | calibrated_ = true; // This is a slow step that we only want to do once. 222 | ROS_INFO("Calibration finished. Latency is: %0.4f", latency); 223 | } 224 | else 225 | { 226 | calibrated_ = false; 227 | laser_.clearLatency(); 228 | } 229 | 230 | setStatusMessage("Device opened successfully.", true); 231 | laser_.getConfig(laser_config_); 232 | 233 | state_ = OPENED; 234 | } 235 | catch (hokuyo::Exception& e) 236 | { 237 | doClose(); 238 | setStatusMessagef("Exception thrown while opening Hokuyo.\n%s", e.what()); 239 | return; 240 | } 241 | } 242 | 243 | void doClose() 244 | { 245 | try 246 | { 247 | laser_.close(); 248 | setStatusMessage("Device closed successfully.", true); 249 | } catch (hokuyo::Exception& e) { 250 | setStatusMessagef("Exception thrown while trying to close:\n%s",e.what()); 251 | } 252 | 253 | state_ = CLOSED; // If we can't close, we are done for anyways. 254 | } 255 | 256 | void doStart() 257 | { 258 | try 259 | { 260 | laser_.laserOn(); 261 | 262 | int status = laser_.requestScans(config_.intensity, config_.min_ang, config_.max_ang, config_.cluster, config_.skip); 263 | 264 | if (status != 0) { 265 | setStatusMessagef("Failed to request scans from device. Status: %d.", status); 266 | corrupted_scan_count_++; 267 | return; 268 | } 269 | 270 | setStatusMessagef("Waiting for first scan.", true); 271 | state_ = RUNNING; 272 | scan_thread_.reset(new boost::thread(boost::bind(&HokuyoDriver::scanThread, this))); 273 | } 274 | catch (hokuyo::Exception& e) 275 | { 276 | doClose(); 277 | setStatusMessagef("Exception thrown while starting Hokuyo.\n%s", e.what()); 278 | connect_fail_ = e.what(); 279 | return; 280 | } 281 | } 282 | 283 | void doStop() 284 | { 285 | if (state_ != RUNNING) // RUNNING can exit asynchronously. 286 | return; 287 | 288 | state_ = OPENED; 289 | 290 | if (scan_thread_ && !scan_thread_->timed_join((boost::posix_time::milliseconds) 2000)) 291 | { 292 | ROS_ERROR("scan_thread_ did not die after two seconds. Pretending that it did. This is probably a bad sign."); 293 | lost_scan_thread_count_++; 294 | } 295 | scan_thread_.reset(); 296 | 297 | setStatusMessagef("Stopped.", true); 298 | } 299 | 300 | virtual std::string getID() 301 | { 302 | std::string id = laser_.getID(); 303 | if (id == std::string("H0000000")) 304 | return "unknown"; 305 | return id; 306 | } 307 | 308 | void config_update(Config &new_config, int level = 0) 309 | { 310 | ROS_DEBUG("Reconfigure called from state %i", state_); 311 | 312 | if (state_ == OPENED) 313 | // If it is closed, we don't know what to check for. If it is running those parameters haven't changed, 314 | // and talking to the hokuyo would cause loads of trouble. 315 | { 316 | checkIntensitySupport(new_config); 317 | checkAngleRange(new_config); 318 | } 319 | 320 | config_ = new_config; 321 | } 322 | 323 | void scanThread() 324 | { 325 | while (state_ == RUNNING) 326 | { 327 | try 328 | { 329 | int status = laser_.serviceScan(scan_); 330 | 331 | if(status != 0) 332 | { 333 | ROS_WARN("Error getting scan: %d", status); 334 | break; 335 | } 336 | } catch (hokuyo::CorruptedDataException &e) { 337 | ROS_WARN("Skipping corrupted data"); 338 | continue; 339 | } catch (hokuyo::Exception& e) { 340 | ROS_WARN("Exception thrown while trying to get scan.\n%s", e.what()); 341 | doClose(); 342 | return; 343 | } 344 | 345 | if (first_scan_) 346 | { 347 | first_scan_ = false; 348 | setStatusMessage("Streaming data.", true, true); 349 | ROS_INFO("Streaming data."); 350 | } 351 | 352 | useScan_(scan_); 353 | } 354 | 355 | try 356 | { 357 | laser_.stopScanning(); // This actually just calls laser Off internally. 358 | } catch (hokuyo::Exception &e) 359 | { 360 | ROS_WARN("Exception thrown while trying to stop scan.\n%s", e.what()); 361 | } 362 | state_ = OPENED; 363 | } 364 | }; 365 | 366 | class HokuyoNode : public driver_base::DriverNode 367 | { 368 | private: 369 | string connect_fail_; 370 | 371 | double desired_freq_; 372 | 373 | ros::NodeHandle node_handle_; 374 | diagnostic_updater::DiagnosedPublisher scan_pub_; 375 | sensor_msgs::LaserScan scan_msg_; 376 | diagnostic_updater::FunctionDiagnosticTask hokuyo_diagnostic_task_; 377 | 378 | public: 379 | HokuyoNode(ros::NodeHandle &nh) : 380 | driver_base::DriverNode(nh), 381 | node_handle_(nh), 382 | scan_pub_(node_handle_.advertise("scan", 100), 383 | diagnostic_, 384 | diagnostic_updater::FrequencyStatusParam(&desired_freq_, &desired_freq_, 0.05), 385 | diagnostic_updater::TimeStampStatusParam()), 386 | hokuyo_diagnostic_task_("Hokuyo Diagnostics", boost::bind(&HokuyoNode::connectionStatus, this, _1)) 387 | { 388 | desired_freq_ = 0; 389 | driver_.useScan_ = boost::bind(&HokuyoNode::publishScan, this, _1); 390 | driver_.setPostOpenHook(boost::bind(&HokuyoNode::postOpenHook, this)); 391 | 392 | // Check if use_rep_117 is set. 393 | std::string key; 394 | if (nh.searchParam("use_rep_117", key)) 395 | { 396 | ROS_ERROR("The use_rep_117 parameter has not been specified and is now ignored. This parameter was removed in Hydromedusa. Please see: http://ros.org/wiki/rep_117/migration"); 397 | } 398 | } 399 | 400 | void postOpenHook() 401 | { 402 | private_node_handle_.setParam("min_ang_limit", (double) (driver_.laser_config_.min_angle)); 403 | private_node_handle_.setParam("max_ang_limit", (double) (driver_.laser_config_.max_angle)); 404 | private_node_handle_.setParam("min_range", (double) (driver_.laser_config_.min_range)); 405 | private_node_handle_.setParam("max_range", (double) (driver_.laser_config_.max_range)); 406 | 407 | diagnostic_.setHardwareID(driver_.getID()); 408 | 409 | if (driver_.checkIntensitySupport(driver_.config_) || 410 | driver_.checkAngleRange(driver_.config_)) // Might have been set before the device's range was known. 411 | reconfigure_server_.updateConfig(driver_.config_); 412 | 413 | scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic. 414 | } 415 | 416 | virtual void addOpenedTests() 417 | { 418 | self_test_.add( "Status Test", this, &HokuyoNode::statusTest ); 419 | self_test_.add( "Laser Test", this, &HokuyoNode::laserTest ); 420 | self_test_.add( "Polled Data Test", this, &HokuyoNode::polledDataTest ); 421 | self_test_.add( "Streamed Data Test", this, &HokuyoNode::streamedDataTest ); 422 | self_test_.add( "Streamed Intensity Data Test", this, &HokuyoNode::streamedIntensityDataTest ); 423 | self_test_.add( "Laser Off Test", this, &HokuyoNode::laserOffTest ); 424 | } 425 | 426 | virtual void addStoppedTests() 427 | { 428 | } 429 | 430 | virtual void addRunningTests() 431 | { 432 | } 433 | 434 | virtual void addDiagnostics() 435 | { 436 | driver_status_diagnostic_.addTask(&hokuyo_diagnostic_task_); 437 | } 438 | 439 | void reconfigureHook(int level) 440 | { 441 | if (private_node_handle_.hasParam("frameid")) 442 | { 443 | ROS_WARN("~frameid is deprecated, please use ~frame_id instead"); 444 | private_node_handle_.getParam("frameid", driver_.config_.frame_id); 445 | } 446 | 447 | if (private_node_handle_.hasParam("min_ang_degrees")) 448 | { 449 | ROS_WARN("~min_ang_degrees is deprecated, please use ~min_ang instead"); 450 | private_node_handle_.getParam("min_ang_degrees", driver_.config_.min_ang); 451 | driver_.config_.min_ang *= M_PI/180; 452 | } 453 | 454 | if (private_node_handle_.hasParam("max_ang_degrees")) 455 | { 456 | ROS_WARN("~max_ang_degrees is deprecated, please use ~max_ang instead"); 457 | private_node_handle_.getParam("max_ang_degrees", driver_.config_.max_ang); 458 | driver_.config_.max_ang *= M_PI/180; 459 | } 460 | 461 | diagnostic_.force_update(); 462 | 463 | scan_pub_.clear_window(); // Reduce glitches in the frequency diagnostic. 464 | } 465 | 466 | int publishScan(const hokuyo::LaserScan &scan) 467 | { 468 | //ROS_DEBUG("publishScan"); 469 | 470 | scan_msg_.angle_min = scan.config.min_angle; 471 | scan_msg_.angle_max = scan.config.max_angle; 472 | scan_msg_.angle_increment = scan.config.ang_increment; 473 | scan_msg_.time_increment = scan.config.time_increment; 474 | scan_msg_.scan_time = scan.config.scan_time; 475 | scan_msg_.range_min = scan.config.min_range; 476 | scan_msg_.range_max = scan.config.max_range; 477 | scan_msg_.ranges = scan.ranges; 478 | scan_msg_.intensities = scan.intensities; 479 | scan_msg_.header.stamp = ros::Time().fromNSec((uint64_t)scan.system_time_stamp) + ros::Duration(driver_.config_.time_offset); 480 | scan_msg_.header.frame_id = driver_.config_.frame_id; 481 | 482 | desired_freq_ = (1. / scan.config.scan_time); 483 | 484 | scan_pub_.publish(scan_msg_); 485 | 486 | //ROS_DEBUG("publishScan done"); 487 | 488 | return(0); 489 | } 490 | 491 | void connectionStatus(diagnostic_updater::DiagnosticStatusWrapper& status) 492 | { 493 | if (driver_.state_ == driver_.CLOSED) 494 | status.summary(2, "Not connected. " + driver_.device_status_ + " " + connect_fail_); 495 | else if (driver_.device_status_ != std::string("Sensor works well.")) 496 | status.summaryf(2, "Abnormal status: %s", driver_.device_status_.c_str()); 497 | else if (driver_.state_ == driver_.RUNNING) 498 | { 499 | if (driver_.first_scan_) 500 | status.summary(0, "Waiting for first scan"); 501 | else 502 | status.summary(0, "Streaming"); 503 | } 504 | else if (driver_.state_ == driver_.OPENED) 505 | status.summary(0, "Open"); 506 | else 507 | status.summary(2, "Unknown state"); 508 | 509 | status.add("Device Status", driver_.device_status_); 510 | status.add("Port", driver_.config_.port); 511 | status.add("Device ID", driver_.device_id_); 512 | status.add("Scan Thread Lost Count", driver_.lost_scan_thread_count_); 513 | status.add("Corrupted Scan Count", driver_.corrupted_scan_count_); 514 | status.add("Vendor Name", driver_.vendor_name_); 515 | status.add("Product Name", driver_.product_name_); 516 | status.add("Firmware Version", driver_.firmware_version_); 517 | status.add("Protocol Version", driver_.protocol_version_); 518 | status.add("Computed Latency", driver_.laser_.getLatency()); 519 | status.add("User Time Offset", driver_.config_.time_offset); 520 | } 521 | 522 | void statusTest(diagnostic_updater::DiagnosticStatusWrapper& status) 523 | { 524 | std::string stat = driver_.laser_.getStatus(); 525 | 526 | if (stat != std::string("Sensor works well.")) 527 | { 528 | status.level = 2; 529 | } else { 530 | status.level = 0; 531 | } 532 | 533 | status.message = stat; 534 | } 535 | 536 | void laserTest(diagnostic_updater::DiagnosticStatusWrapper& status) 537 | { 538 | driver_.laser_.laserOn(); 539 | 540 | status.level = 0; 541 | status.message = "Laser turned on successfully."; 542 | } 543 | 544 | void polledDataTest(diagnostic_updater::DiagnosticStatusWrapper& status) 545 | { 546 | hokuyo::LaserScan scan; 547 | 548 | int res = driver_.laser_.pollScan(scan, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1000); 549 | 550 | if (res != 0) 551 | { 552 | status.level = 2; 553 | ostringstream oss; 554 | oss << "Hokuyo error code: " << res << ". Consult manual for meaning."; 555 | status.message = oss.str(); 556 | 557 | } else { 558 | status.level = 0; 559 | status.message = "Polled Hokuyo for data successfully."; 560 | } 561 | } 562 | 563 | void streamedDataTest(diagnostic_updater::DiagnosticStatusWrapper& status) 564 | { 565 | hokuyo::LaserScan scan; 566 | 567 | int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000); 568 | 569 | if (res != 0) 570 | { 571 | status.level = 2; 572 | ostringstream oss; 573 | oss << "Hokuyo error code: " << res << ". Consult manual for meaning."; 574 | status.message = oss.str(); 575 | 576 | } else { 577 | 578 | for (int i = 0; i < 99; i++) 579 | { 580 | driver_.laser_.serviceScan(scan, 1000); 581 | } 582 | 583 | status.level = 0; 584 | status.message = "Streamed data from Hokuyo successfully."; 585 | 586 | } 587 | } 588 | 589 | void streamedIntensityDataTest(diagnostic_updater::DiagnosticStatusWrapper& status) 590 | { 591 | hokuyo::LaserScan scan; 592 | 593 | int res = driver_.laser_.requestScans(false, driver_.laser_config_.min_angle, driver_.laser_config_.max_angle, 1, 1, 99, 1000); 594 | 595 | if (res != 0) 596 | { 597 | status.level = 2; 598 | ostringstream oss; 599 | oss << "Hokuyo error code: " << res << ". Consult manual for meaning."; 600 | status.message = oss.str(); 601 | 602 | } else { 603 | 604 | int corrupted_data = 0; 605 | 606 | for (int i = 0; i < 99; i++) 607 | { 608 | try { 609 | driver_.laser_.serviceScan(scan, 1000); 610 | } catch (hokuyo::CorruptedDataException &e) { 611 | corrupted_data++; 612 | } 613 | } 614 | if (corrupted_data == 1) 615 | { 616 | status.level = 1; 617 | status.message = "Single corrupted message. This is acceptable and unavoidable"; 618 | } else if (corrupted_data > 1) 619 | { 620 | status.level = 2; 621 | ostringstream oss; 622 | oss << corrupted_data << " corrupted messages."; 623 | status.message = oss.str(); 624 | } else 625 | { 626 | status.level = 0; 627 | status.message = "Stramed data with intensity from Hokuyo successfully."; 628 | } 629 | } 630 | } 631 | 632 | void laserOffTest(diagnostic_updater::DiagnosticStatusWrapper& status) 633 | { 634 | driver_.laser_.laserOff(); 635 | 636 | status.level = 0; 637 | status.message = "Laser turned off successfully."; 638 | } 639 | }; 640 | 641 | int main(int argc, char **argv) 642 | { 643 | return driver_base::main(argc, argv, "hokuyo_node"); 644 | } 645 | 646 | -------------------------------------------------------------------------------- /COPYING.lib: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 2.1, February 1999 3 | 4 | Copyright (C) 1991, 1999 Free Software Foundation, Inc. 5 | 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 6 | Everyone is permitted to copy and distribute verbatim copies 7 | of this license document, but changing it is not allowed. 8 | 9 | [This is the first released version of the Lesser GPL. It also counts 10 | as the successor of the GNU Library Public License, version 2, hence 11 | the version number 2.1.] 12 | 13 | Preamble 14 | 15 | The licenses for most software are designed to take away your 16 | freedom to share and change it. By contrast, the GNU General Public 17 | Licenses are intended to guarantee your freedom to share and change 18 | free software--to make sure the software is free for all its users. 19 | 20 | This license, the Lesser General Public License, applies to some 21 | specially designated software packages--typically libraries--of the 22 | Free Software Foundation and other authors who decide to use it. You 23 | can use it too, but we suggest you first think carefully about whether 24 | this license or the ordinary General Public License is the better 25 | strategy to use in any particular case, based on the explanations below. 26 | 27 | When we speak of free software, we are referring to freedom of use, 28 | not price. Our General Public Licenses are designed to make sure that 29 | you have the freedom to distribute copies of free software (and charge 30 | for this service if you wish); that you receive source code or can get 31 | it if you want it; that you can change the software and use pieces of 32 | it in new free programs; and that you are informed that you can do 33 | these things. 34 | 35 | To protect your rights, we need to make restrictions that forbid 36 | distributors to deny you these rights or to ask you to surrender these 37 | rights. These restrictions translate to certain responsibilities for 38 | you if you distribute copies of the library or if you modify it. 39 | 40 | For example, if you distribute copies of the library, whether gratis 41 | or for a fee, you must give the recipients all the rights that we gave 42 | you. You must make sure that they, too, receive or can get the source 43 | code. If you link other code with the library, you must provide 44 | complete object files to the recipients, so that they can relink them 45 | with the library after making changes to the library and recompiling 46 | it. And you must show them these terms so they know their rights. 47 | 48 | We protect your rights with a two-step method: (1) we copyright the 49 | library, and (2) we offer you this license, which gives you legal 50 | permission to copy, distribute and/or modify the library. 51 | 52 | To protect each distributor, we want to make it very clear that 53 | there is no warranty for the free library. Also, if the library is 54 | modified by someone else and passed on, the recipients should know 55 | that what they have is not the original version, so that the original 56 | author's reputation will not be affected by problems that might be 57 | introduced by others. 58 | 59 | Finally, software patents pose a constant threat to the existence of 60 | any free program. We wish to make sure that a company cannot 61 | effectively restrict the users of a free program by obtaining a 62 | restrictive license from a patent holder. Therefore, we insist that 63 | any patent license obtained for a version of the library must be 64 | consistent with the full freedom of use specified in this license. 65 | 66 | Most GNU software, including some libraries, is covered by the 67 | ordinary GNU General Public License. This license, the GNU Lesser 68 | General Public License, applies to certain designated libraries, and 69 | is quite different from the ordinary General Public License. We use 70 | this license for certain libraries in order to permit linking those 71 | libraries into non-free programs. 72 | 73 | When a program is linked with a library, whether statically or using 74 | a shared library, the combination of the two is legally speaking a 75 | combined work, a derivative of the original library. The ordinary 76 | General Public License therefore permits such linking only if the 77 | entire combination fits its criteria of freedom. The Lesser General 78 | Public License permits more lax criteria for linking other code with 79 | the library. 80 | 81 | We call this license the "Lesser" General Public License because it 82 | does Less to protect the user's freedom than the ordinary General 83 | Public License. It also provides other free software developers Less 84 | of an advantage over competing non-free programs. These disadvantages 85 | are the reason we use the ordinary General Public License for many 86 | libraries. However, the Lesser license provides advantages in certain 87 | special circumstances. 88 | 89 | For example, on rare occasions, there may be a special need to 90 | encourage the widest possible use of a certain library, so that it becomes 91 | a de-facto standard. To achieve this, non-free programs must be 92 | allowed to use the library. A more frequent case is that a free 93 | library does the same job as widely used non-free libraries. In this 94 | case, there is little to gain by limiting the free library to free 95 | software only, so we use the Lesser General Public License. 96 | 97 | In other cases, permission to use a particular library in non-free 98 | programs enables a greater number of people to use a large body of 99 | free software. For example, permission to use the GNU C Library in 100 | non-free programs enables many more people to use the whole GNU 101 | operating system, as well as its variant, the GNU/Linux operating 102 | system. 103 | 104 | Although the Lesser General Public License is Less protective of the 105 | users' freedom, it does ensure that the user of a program that is 106 | linked with the Library has the freedom and the wherewithal to run 107 | that program using a modified version of the Library. 108 | 109 | The precise terms and conditions for copying, distribution and 110 | modification follow. Pay close attention to the difference between a 111 | "work based on the library" and a "work that uses the library". The 112 | former contains code derived from the library, whereas the latter must 113 | be combined with the library in order to run. 114 | 115 | GNU LESSER GENERAL PUBLIC LICENSE 116 | TERMS AND CONDITIONS FOR COPYING, DISTRIBUTION AND MODIFICATION 117 | 118 | 0. This License Agreement applies to any software library or other 119 | program which contains a notice placed by the copyright holder or 120 | other authorized party saying it may be distributed under the terms of 121 | this Lesser General Public License (also called "this License"). 122 | Each licensee is addressed as "you". 123 | 124 | A "library" means a collection of software functions and/or data 125 | prepared so as to be conveniently linked with application programs 126 | (which use some of those functions and data) to form executables. 127 | 128 | The "Library", below, refers to any such software library or work 129 | which has been distributed under these terms. A "work based on the 130 | Library" means either the Library or any derivative work under 131 | copyright law: that is to say, a work containing the Library or a 132 | portion of it, either verbatim or with modifications and/or translated 133 | straightforwardly into another language. (Hereinafter, translation is 134 | included without limitation in the term "modification".) 135 | 136 | "Source code" for a work means the preferred form of the work for 137 | making modifications to it. For a library, complete source code means 138 | all the source code for all modules it contains, plus any associated 139 | interface definition files, plus the scripts used to control compilation 140 | and installation of the library. 141 | 142 | Activities other than copying, distribution and modification are not 143 | covered by this License; they are outside its scope. The act of 144 | running a program using the Library is not restricted, and output from 145 | such a program is covered only if its contents constitute a work based 146 | on the Library (independent of the use of the Library in a tool for 147 | writing it). Whether that is true depends on what the Library does 148 | and what the program that uses the Library does. 149 | 150 | 1. You may copy and distribute verbatim copies of the Library's 151 | complete source code as you receive it, in any medium, provided that 152 | you conspicuously and appropriately publish on each copy an 153 | appropriate copyright notice and disclaimer of warranty; keep intact 154 | all the notices that refer to this License and to the absence of any 155 | warranty; and distribute a copy of this License along with the 156 | Library. 157 | 158 | You may charge a fee for the physical act of transferring a copy, 159 | and you may at your option offer warranty protection in exchange for a 160 | fee. 161 | 162 | 2. You may modify your copy or copies of the Library or any portion 163 | of it, thus forming a work based on the Library, and copy and 164 | distribute such modifications or work under the terms of Section 1 165 | above, provided that you also meet all of these conditions: 166 | 167 | a) The modified work must itself be a software library. 168 | 169 | b) You must cause the files modified to carry prominent notices 170 | stating that you changed the files and the date of any change. 171 | 172 | c) You must cause the whole of the work to be licensed at no 173 | charge to all third parties under the terms of this License. 174 | 175 | d) If a facility in the modified Library refers to a function or a 176 | table of data to be supplied by an application program that uses 177 | the facility, other than as an argument passed when the facility 178 | is invoked, then you must make a good faith effort to ensure that, 179 | in the event an application does not supply such function or 180 | table, the facility still operates, and performs whatever part of 181 | its purpose remains meaningful. 182 | 183 | (For example, a function in a library to compute square roots has 184 | a purpose that is entirely well-defined independent of the 185 | application. Therefore, Subsection 2d requires that any 186 | application-supplied function or table used by this function must 187 | be optional: if the application does not supply it, the square 188 | root function must still compute square roots.) 189 | 190 | These requirements apply to the modified work as a whole. If 191 | identifiable sections of that work are not derived from the Library, 192 | and can be reasonably considered independent and separate works in 193 | themselves, then this License, and its terms, do not apply to those 194 | sections when you distribute them as separate works. But when you 195 | distribute the same sections as part of a whole which is a work based 196 | on the Library, the distribution of the whole must be on the terms of 197 | this License, whose permissions for other licensees extend to the 198 | entire whole, and thus to each and every part regardless of who wrote 199 | it. 200 | 201 | Thus, it is not the intent of this section to claim rights or contest 202 | your rights to work written entirely by you; rather, the intent is to 203 | exercise the right to control the distribution of derivative or 204 | collective works based on the Library. 205 | 206 | In addition, mere aggregation of another work not based on the Library 207 | with the Library (or with a work based on the Library) on a volume of 208 | a storage or distribution medium does not bring the other work under 209 | the scope of this License. 210 | 211 | 3. You may opt to apply the terms of the ordinary GNU General Public 212 | License instead of this License to a given copy of the Library. To do 213 | this, you must alter all the notices that refer to this License, so 214 | that they refer to the ordinary GNU General Public License, version 2, 215 | instead of to this License. (If a newer version than version 2 of the 216 | ordinary GNU General Public License has appeared, then you can specify 217 | that version instead if you wish.) Do not make any other change in 218 | these notices. 219 | 220 | Once this change is made in a given copy, it is irreversible for 221 | that copy, so the ordinary GNU General Public License applies to all 222 | subsequent copies and derivative works made from that copy. 223 | 224 | This option is useful when you wish to copy part of the code of 225 | the Library into a program that is not a library. 226 | 227 | 4. You may copy and distribute the Library (or a portion or 228 | derivative of it, under Section 2) in object code or executable form 229 | under the terms of Sections 1 and 2 above provided that you accompany 230 | it with the complete corresponding machine-readable source code, which 231 | must be distributed under the terms of Sections 1 and 2 above on a 232 | medium customarily used for software interchange. 233 | 234 | If distribution of object code is made by offering access to copy 235 | from a designated place, then offering equivalent access to copy the 236 | source code from the same place satisfies the requirement to 237 | distribute the source code, even though third parties are not 238 | compelled to copy the source along with the object code. 239 | 240 | 5. A program that contains no derivative of any portion of the 241 | Library, but is designed to work with the Library by being compiled or 242 | linked with it, is called a "work that uses the Library". Such a 243 | work, in isolation, is not a derivative work of the Library, and 244 | therefore falls outside the scope of this License. 245 | 246 | However, linking a "work that uses the Library" with the Library 247 | creates an executable that is a derivative of the Library (because it 248 | contains portions of the Library), rather than a "work that uses the 249 | library". The executable is therefore covered by this License. 250 | Section 6 states terms for distribution of such executables. 251 | 252 | When a "work that uses the Library" uses material from a header file 253 | that is part of the Library, the object code for the work may be a 254 | derivative work of the Library even though the source code is not. 255 | Whether this is true is especially significant if the work can be 256 | linked without the Library, or if the work is itself a library. The 257 | threshold for this to be true is not precisely defined by law. 258 | 259 | If such an object file uses only numerical parameters, data 260 | structure layouts and accessors, and small macros and small inline 261 | functions (ten lines or less in length), then the use of the object 262 | file is unrestricted, regardless of whether it is legally a derivative 263 | work. (Executables containing this object code plus portions of the 264 | Library will still fall under Section 6.) 265 | 266 | Otherwise, if the work is a derivative of the Library, you may 267 | distribute the object code for the work under the terms of Section 6. 268 | Any executables containing that work also fall under Section 6, 269 | whether or not they are linked directly with the Library itself. 270 | 271 | 6. As an exception to the Sections above, you may also combine or 272 | link a "work that uses the Library" with the Library to produce a 273 | work containing portions of the Library, and distribute that work 274 | under terms of your choice, provided that the terms permit 275 | modification of the work for the customer's own use and reverse 276 | engineering for debugging such modifications. 277 | 278 | You must give prominent notice with each copy of the work that the 279 | Library is used in it and that the Library and its use are covered by 280 | this License. You must supply a copy of this License. If the work 281 | during execution displays copyright notices, you must include the 282 | copyright notice for the Library among them, as well as a reference 283 | directing the user to the copy of this License. Also, you must do one 284 | of these things: 285 | 286 | a) Accompany the work with the complete corresponding 287 | machine-readable source code for the Library including whatever 288 | changes were used in the work (which must be distributed under 289 | Sections 1 and 2 above); and, if the work is an executable linked 290 | with the Library, with the complete machine-readable "work that 291 | uses the Library", as object code and/or source code, so that the 292 | user can modify the Library and then relink to produce a modified 293 | executable containing the modified Library. (It is understood 294 | that the user who changes the contents of definitions files in the 295 | Library will not necessarily be able to recompile the application 296 | to use the modified definitions.) 297 | 298 | b) Use a suitable shared library mechanism for linking with the 299 | Library. A suitable mechanism is one that (1) uses at run time a 300 | copy of the library already present on the user's computer system, 301 | rather than copying library functions into the executable, and (2) 302 | will operate properly with a modified version of the library, if 303 | the user installs one, as long as the modified version is 304 | interface-compatible with the version that the work was made with. 305 | 306 | c) Accompany the work with a written offer, valid for at 307 | least three years, to give the same user the materials 308 | specified in Subsection 6a, above, for a charge no more 309 | than the cost of performing this distribution. 310 | 311 | d) If distribution of the work is made by offering access to copy 312 | from a designated place, offer equivalent access to copy the above 313 | specified materials from the same place. 314 | 315 | e) Verify that the user has already received a copy of these 316 | materials or that you have already sent this user a copy. 317 | 318 | For an executable, the required form of the "work that uses the 319 | Library" must include any data and utility programs needed for 320 | reproducing the executable from it. However, as a special exception, 321 | the materials to be distributed need not include anything that is 322 | normally distributed (in either source or binary form) with the major 323 | components (compiler, kernel, and so on) of the operating system on 324 | which the executable runs, unless that component itself accompanies 325 | the executable. 326 | 327 | It may happen that this requirement contradicts the license 328 | restrictions of other proprietary libraries that do not normally 329 | accompany the operating system. Such a contradiction means you cannot 330 | use both them and the Library together in an executable that you 331 | distribute. 332 | 333 | 7. You may place library facilities that are a work based on the 334 | Library side-by-side in a single library together with other library 335 | facilities not covered by this License, and distribute such a combined 336 | library, provided that the separate distribution of the work based on 337 | the Library and of the other library facilities is otherwise 338 | permitted, and provided that you do these two things: 339 | 340 | a) Accompany the combined library with a copy of the same work 341 | based on the Library, uncombined with any other library 342 | facilities. This must be distributed under the terms of the 343 | Sections above. 344 | 345 | b) Give prominent notice with the combined library of the fact 346 | that part of it is a work based on the Library, and explaining 347 | where to find the accompanying uncombined form of the same work. 348 | 349 | 8. You may not copy, modify, sublicense, link with, or distribute 350 | the Library except as expressly provided under this License. Any 351 | attempt otherwise to copy, modify, sublicense, link with, or 352 | distribute the Library is void, and will automatically terminate your 353 | rights under this License. However, parties who have received copies, 354 | or rights, from you under this License will not have their licenses 355 | terminated so long as such parties remain in full compliance. 356 | 357 | 9. You are not required to accept this License, since you have not 358 | signed it. However, nothing else grants you permission to modify or 359 | distribute the Library or its derivative works. These actions are 360 | prohibited by law if you do not accept this License. Therefore, by 361 | modifying or distributing the Library (or any work based on the 362 | Library), you indicate your acceptance of this License to do so, and 363 | all its terms and conditions for copying, distributing or modifying 364 | the Library or works based on it. 365 | 366 | 10. Each time you redistribute the Library (or any work based on the 367 | Library), the recipient automatically receives a license from the 368 | original licensor to copy, distribute, link with or modify the Library 369 | subject to these terms and conditions. You may not impose any further 370 | restrictions on the recipients' exercise of the rights granted herein. 371 | You are not responsible for enforcing compliance by third parties with 372 | this License. 373 | 374 | 11. If, as a consequence of a court judgment or allegation of patent 375 | infringement or for any other reason (not limited to patent issues), 376 | conditions are imposed on you (whether by court order, agreement or 377 | otherwise) that contradict the conditions of this License, they do not 378 | excuse you from the conditions of this License. If you cannot 379 | distribute so as to satisfy simultaneously your obligations under this 380 | License and any other pertinent obligations, then as a consequence you 381 | may not distribute the Library at all. For example, if a patent 382 | license would not permit royalty-free redistribution of the Library by 383 | all those who receive copies directly or indirectly through you, then 384 | the only way you could satisfy both it and this License would be to 385 | refrain entirely from distribution of the Library. 386 | 387 | If any portion of this section is held invalid or unenforceable under any 388 | particular circumstance, the balance of the section is intended to apply, 389 | and the section as a whole is intended to apply in other circumstances. 390 | 391 | It is not the purpose of this section to induce you to infringe any 392 | patents or other property right claims or to contest validity of any 393 | such claims; this section has the sole purpose of protecting the 394 | integrity of the free software distribution system which is 395 | implemented by public license practices. Many people have made 396 | generous contributions to the wide range of software distributed 397 | through that system in reliance on consistent application of that 398 | system; it is up to the author/donor to decide if he or she is willing 399 | to distribute software through any other system and a licensee cannot 400 | impose that choice. 401 | 402 | This section is intended to make thoroughly clear what is believed to 403 | be a consequence of the rest of this License. 404 | 405 | 12. If the distribution and/or use of the Library is restricted in 406 | certain countries either by patents or by copyrighted interfaces, the 407 | original copyright holder who places the Library under this License may add 408 | an explicit geographical distribution limitation excluding those countries, 409 | so that distribution is permitted only in or among countries not thus 410 | excluded. In such case, this License incorporates the limitation as if 411 | written in the body of this License. 412 | 413 | 13. The Free Software Foundation may publish revised and/or new 414 | versions of the Lesser General Public License from time to time. 415 | Such new versions will be similar in spirit to the present version, 416 | but may differ in detail to address new problems or concerns. 417 | 418 | Each version is given a distinguishing version number. If the Library 419 | specifies a version number of this License which applies to it and 420 | "any later version", you have the option of following the terms and 421 | conditions either of that version or of any later version published by 422 | the Free Software Foundation. If the Library does not specify a 423 | license version number, you may choose any version ever published by 424 | the Free Software Foundation. 425 | 426 | 14. If you wish to incorporate parts of the Library into other free 427 | programs whose distribution conditions are incompatible with these, 428 | write to the author to ask for permission. For software which is 429 | copyrighted by the Free Software Foundation, write to the Free 430 | Software Foundation; we sometimes make exceptions for this. Our 431 | decision will be guided by the two goals of preserving the free status 432 | of all derivatives of our free software and of promoting the sharing 433 | and reuse of software generally. 434 | 435 | NO WARRANTY 436 | 437 | 15. BECAUSE THE LIBRARY IS LICENSED FREE OF CHARGE, THERE IS NO 438 | WARRANTY FOR THE LIBRARY, TO THE EXTENT PERMITTED BY APPLICABLE LAW. 439 | EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT HOLDERS AND/OR 440 | OTHER PARTIES PROVIDE THE LIBRARY "AS IS" WITHOUT WARRANTY OF ANY 441 | KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, THE 442 | IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR 443 | PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE 444 | LIBRARY IS WITH YOU. SHOULD THE LIBRARY PROVE DEFECTIVE, YOU ASSUME 445 | THE COST OF ALL NECESSARY SERVICING, REPAIR OR CORRECTION. 446 | 447 | 16. IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN 448 | WRITING WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MAY MODIFY 449 | AND/OR REDISTRIBUTE THE LIBRARY AS PERMITTED ABOVE, BE LIABLE TO YOU 450 | FOR DAMAGES, INCLUDING ANY GENERAL, SPECIAL, INCIDENTAL OR 451 | CONSEQUENTIAL DAMAGES ARISING OUT OF THE USE OR INABILITY TO USE THE 452 | LIBRARY (INCLUDING BUT NOT LIMITED TO LOSS OF DATA OR DATA BEING 453 | RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD PARTIES OR A 454 | FAILURE OF THE LIBRARY TO OPERATE WITH ANY OTHER SOFTWARE), EVEN IF 455 | SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF SUCH 456 | DAMAGES. 457 | 458 | END OF TERMS AND CONDITIONS 459 | 460 | How to Apply These Terms to Your New Libraries 461 | 462 | If you develop a new library, and you want it to be of the greatest 463 | possible use to the public, we recommend making it free software that 464 | everyone can redistribute and change. You can do so by permitting 465 | redistribution under these terms (or, alternatively, under the terms of the 466 | ordinary General Public License). 467 | 468 | To apply these terms, attach the following notices to the library. It is 469 | safest to attach them to the start of each source file to most effectively 470 | convey the exclusion of warranty; and each file should have at least the 471 | "copyright" line and a pointer to where the full notice is found. 472 | 473 | 474 | Copyright (C) 475 | 476 | This library is free software; you can redistribute it and/or 477 | modify it under the terms of the GNU Lesser General Public 478 | License as published by the Free Software Foundation; either 479 | version 2.1 of the License, or (at your option) any later version. 480 | 481 | This library is distributed in the hope that it will be useful, 482 | but WITHOUT ANY WARRANTY; without even the implied warranty of 483 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 484 | Lesser General Public License for more details. 485 | 486 | You should have received a copy of the GNU Lesser General Public 487 | License along with this library; if not, write to the Free Software 488 | Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA 489 | 490 | Also add information on how to contact you by electronic and paper mail. 491 | 492 | You should also get your employer (if you work as a programmer) or your 493 | school, if any, to sign a "copyright disclaimer" for the library, if 494 | necessary. Here is a sample; alter the names: 495 | 496 | Yoyodyne, Inc., hereby disclaims all copyright interest in the 497 | library `Frob' (a library for tweaking knobs) written by James Random Hacker. 498 | 499 | , 1 April 1990 500 | Ty Coon, President of Vice 501 | 502 | That's all there is to it! 503 | 504 | 505 | -------------------------------------------------------------------------------- /src/hokuyo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Player - One Hell of a Robot Server 3 | * Copyright (C) 2008-2010 Willow Garage 4 | * 5 | * 6 | * This library is free software; you can redistribute it and/or 7 | * modify it under the terms of the GNU Lesser General Public 8 | * License as published by the Free Software Foundation; either 9 | * version 2.1 of the License, or (at your option) any later version. 10 | * 11 | * This library is distributed in the hope that it will be useful, 12 | * but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 14 | * Lesser General Public License for more details. 15 | * 16 | * You should have received a copy of the GNU Lesser General Public 17 | * License along with this library; if not, write to the Free Software 18 | * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA 19 | */ 20 | 21 | 22 | #include 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #include "hokuyo_node/hokuyo.h" 31 | 32 | #include 33 | 34 | #include 35 | 36 | #include 37 | 38 | #include "ros/console.h" 39 | 40 | #if POSIX_TIMERS <= 0 41 | #include 42 | #endif 43 | 44 | 45 | //! Macro for throwing an exception with a message, passing args 46 | #define HOKUYO_EXCEPT(except, msg, ...) \ 47 | { \ 48 | char __buf[1000]; \ 49 | snprintf(__buf, 1000, msg " (in hokuyo::laser::%s) You may find further details at http://www.ros.org/wiki/hokuyo_node/Troubleshooting" , ##__VA_ARGS__, __FUNCTION__); \ 50 | throw except(__buf); \ 51 | } 52 | 53 | 54 | //! Helper function for querying the system time 55 | static uint64_t timeHelper() 56 | { 57 | #if POSIX_TIMERS > 0 58 | struct timespec curtime; 59 | clock_gettime(CLOCK_REALTIME, &curtime); 60 | return (uint64_t)(curtime.tv_sec) * 1000000000 + (uint64_t)(curtime.tv_nsec); 61 | #else 62 | struct timeval timeofday; 63 | gettimeofday(&timeofday,NULL); 64 | return (uint64_t)(timeofday.tv_sec) * 1000000000 + (uint64_t)(timeofday.tv_usec) * 1000; 65 | #endif 66 | } 67 | 68 | 69 | #ifdef USE_LOG_FILE 70 | FILE *logfile; 71 | #endif 72 | 73 | /////////////////////////////////////////////////////////////////////////////// 74 | hokuyo::Laser::Laser() : 75 | dmin_(0), dmax_(0), ares_(0), amin_(0), amax_(0), afrt_(0), rate_(0), 76 | wrapped_(0), last_time_(0), time_repeat_count_(0), offset_(0), 77 | laser_fd_(-1) 78 | { 79 | #ifdef USE_LOG_FILE 80 | if (!logfile) 81 | logfile = fopen("hokuyo.log", "w"); 82 | #endif 83 | } 84 | 85 | 86 | /////////////////////////////////////////////////////////////////////////////// 87 | hokuyo::Laser::~Laser () 88 | { 89 | if (portOpen()) 90 | close(); 91 | } 92 | 93 | 94 | /////////////////////////////////////////////////////////////////////////////// 95 | void 96 | hokuyo::Laser::open(const char * port_name) 97 | { 98 | if (portOpen()) 99 | close(); 100 | 101 | // Make IO non blocking. This way there are no race conditions that 102 | // cause blocking when a badly behaving process does a read at the same 103 | // time as us. Will need to switch to blocking for writes or errors 104 | // occur just after a replug event. 105 | laser_fd_ = ::open(port_name, O_RDWR | O_NONBLOCK | O_NOCTTY); 106 | read_buf_start = read_buf_end = 0; 107 | 108 | if (laser_fd_ == -1) 109 | { 110 | const char *extra_msg = ""; 111 | switch (errno) 112 | { 113 | case EACCES: 114 | extra_msg = "You probably don't have premission to open the port for reading and writing."; 115 | break; 116 | case ENOENT: 117 | extra_msg = "The requested port does not exist. Is the hokuyo connected? Was the port name misspelled?"; 118 | break; 119 | } 120 | 121 | HOKUYO_EXCEPT(hokuyo::Exception, "Failed to open port: %s. %s (errno = %d). %s", port_name, strerror(errno), errno, extra_msg); 122 | } 123 | try 124 | { 125 | struct flock fl; 126 | fl.l_type = F_WRLCK; 127 | fl.l_whence = SEEK_SET; 128 | fl.l_start = 0; 129 | fl.l_len = 0; 130 | fl.l_pid = getpid(); 131 | 132 | if (fcntl(laser_fd_, F_SETLK, &fl) != 0) 133 | HOKUYO_EXCEPT(hokuyo::Exception, "Device %s is already locked. Try 'lsof | grep %s' to find other processes that currently have the port open.", port_name, port_name); 134 | 135 | // Settings for USB? 136 | struct termios newtio; 137 | tcgetattr(laser_fd_, &newtio); 138 | memset (&newtio.c_cc, 0, sizeof (newtio.c_cc)); 139 | newtio.c_cflag = CS8 | CLOCAL | CREAD; 140 | newtio.c_iflag = IGNPAR; 141 | newtio.c_oflag = 0; 142 | newtio.c_lflag = 0; 143 | 144 | // activate new settings 145 | tcflush (laser_fd_, TCIFLUSH); 146 | if (tcsetattr (laser_fd_, TCSANOW, &newtio) < 0) 147 | HOKUYO_EXCEPT(hokuyo::Exception, "Unable to set serial port attributes. The port you specified (%s) may not be a serial port.", port_name); /// @todo tcsetattr returns true if at least one attribute was set. Hence, we might not have set everything on success. 148 | usleep (200000); 149 | 150 | // Some models (04LX) need to be told to go into SCIP2 mode... 151 | laserFlush(); 152 | // Just in case a previous failure mode has left our Hokuyo 153 | // spewing data, we send reset the laser to be safe. 154 | try { 155 | reset(); 156 | } 157 | catch (hokuyo::Exception &e) 158 | { 159 | // This might be a device that needs to be explicitely placed in 160 | // SCIP2 mode. 161 | // Note: Not tested: a device that is currently scanning in SCIP1.1 162 | // mode might not manage to switch to SCIP2.0. 163 | 164 | setToSCIP2(); // If this fails then it wasn't a device that could be switched to SCIP2. 165 | reset(); // If this one fails, it really is an error. 166 | } 167 | 168 | querySensorConfig(); 169 | 170 | queryVersionInformation(); // In preparation for calls to get various parts of the version info. 171 | } 172 | catch (hokuyo::Exception& e) 173 | { 174 | // These exceptions mean something failed on open and we should close 175 | if (laser_fd_ != -1) 176 | ::close(laser_fd_); 177 | laser_fd_ = -1; 178 | throw e; 179 | } 180 | } 181 | 182 | 183 | /////////////////////////////////////////////////////////////////////////////// 184 | void hokuyo::Laser::reset () 185 | { 186 | if (!portOpen()) 187 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 188 | laserFlush(); 189 | try 190 | { 191 | sendCmd("TM2", 1000); 192 | } 193 | catch (hokuyo::Exception &e) 194 | {} // Ignore. If the laser was scanning TM2 would fail 195 | try 196 | { 197 | sendCmd("RS", 1000); 198 | last_time_ = 0; // RS resets the hokuyo clock. 199 | wrapped_ = 0; // RS resets the hokuyo clock. 200 | } 201 | catch (hokuyo::Exception &e) 202 | {} // Ignore. If the command coincided with a scan we might get garbage. 203 | laserFlush(); 204 | sendCmd("RS", 1000); // This one should just work. 205 | } 206 | 207 | 208 | /////////////////////////////////////////////////////////////////////////////// 209 | void 210 | hokuyo::Laser::close () 211 | { 212 | int retval = 0; 213 | 214 | if (portOpen()) { 215 | //Try to be a good citizen and completely shut down the laser before we shutdown communication 216 | try 217 | { 218 | reset(); 219 | } 220 | catch (hokuyo::Exception& e) { 221 | //Exceptions here can be safely ignored since we are closing the port anyways 222 | } 223 | 224 | retval = ::close(laser_fd_); // Automatically releases the lock. 225 | } 226 | 227 | laser_fd_ = -1; 228 | 229 | if (retval != 0) 230 | HOKUYO_EXCEPT(hokuyo::Exception, "Failed to close port properly -- error = %d: %s\n", errno, strerror(errno)); 231 | } 232 | 233 | /////////////////////////////////////////////////////////////////////////////// 234 | void 235 | hokuyo::Laser::setToSCIP2() 236 | { 237 | if (!portOpen()) 238 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 239 | const char * cmd = "SCIP2.0"; 240 | char buf[100]; 241 | laserWrite(cmd); 242 | laserWrite("\n"); 243 | 244 | laserReadline(buf, 100, 1000); 245 | ROS_DEBUG("Laser comm protocol changed to %s \n", buf); 246 | //printf ("Laser comm protocol changed to %s \n", buf); 247 | } 248 | 249 | 250 | /////////////////////////////////////////////////////////////////////////////// 251 | int 252 | hokuyo::Laser::sendCmd(const char* cmd, int timeout) 253 | { 254 | if (!portOpen()) 255 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 256 | 257 | char buf[100]; 258 | 259 | laserWrite(cmd); 260 | laserWrite("\n"); 261 | 262 | laserReadlineAfter(buf, 100, cmd, timeout); 263 | laserReadline(buf,100,timeout); 264 | 265 | if (!checkSum(buf,4)) 266 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code."); 267 | 268 | buf[2] = 0; 269 | 270 | if (buf[0] - '0' >= 0 && buf[0] - '0' <= 9 && buf[1] - '0' >= 0 && buf[1] - '0' <= 9) 271 | return (buf[0] - '0')*10 + (buf[1] - '0'); 272 | else 273 | HOKUYO_EXCEPT(hokuyo::Exception, "Hokuyo error code returned. Cmd: %s -- Error: %s", cmd, buf); 274 | } 275 | 276 | 277 | /////////////////////////////////////////////////////////////////////////////// 278 | void 279 | hokuyo::Laser::getConfig(LaserConfig& config) 280 | { 281 | config.min_angle = (amin_ - afrt_) * (2.0*M_PI)/(ares_); 282 | config.max_angle = (amax_ - afrt_) * (2.0*M_PI)/(ares_); 283 | config.ang_increment = (2.0*M_PI)/(ares_); 284 | config.time_increment = (60.0)/(double)(rate_ * ares_); 285 | config.scan_time = 60.0/((double)(rate_)); 286 | config.min_range = dmin_ / 1000.0; 287 | config.max_range = dmax_ / 1000.0; 288 | } 289 | 290 | 291 | /////////////////////////////////////////////////////////////////////////////// 292 | int 293 | hokuyo::Laser::laserWrite(const char* msg) 294 | { 295 | // IO is currently non-blocking. This is what we want for the more common read case. 296 | int origflags = fcntl(laser_fd_,F_GETFL,0); 297 | fcntl(laser_fd_, F_SETFL, origflags & ~O_NONBLOCK); // @todo can we make this all work in non-blocking? 298 | ssize_t len = strlen(msg); 299 | ssize_t retval = write(laser_fd_, msg, len); 300 | int fputserrno = errno; 301 | fcntl(laser_fd_, F_SETFL, origflags | O_NONBLOCK); 302 | errno = fputserrno; // Don't want to see the fcntl errno below. 303 | 304 | if (retval != -1) 305 | { 306 | #ifdef USE_LOG_FILE 307 | if (strlen(msg) > 1) 308 | { 309 | long long outtime = timeHelper(); 310 | fprintf(logfile, "Out: %lli.%09lli %s\n", outtime / 1000000000L, outtime % 1000000000L, msg); 311 | } 312 | #endif 313 | return retval; 314 | } 315 | else 316 | HOKUYO_EXCEPT(hokuyo::Exception, "fputs failed -- Error = %d: %s", errno, strerror(errno)); 317 | } 318 | 319 | 320 | /////////////////////////////////////////////////////////////////////////////// 321 | int 322 | hokuyo::Laser::laserFlush() 323 | { 324 | int retval = tcflush(laser_fd_, TCIOFLUSH); 325 | if (retval != 0) 326 | HOKUYO_EXCEPT(hokuyo::Exception, "tcflush failed"); 327 | read_buf_start = 0; 328 | read_buf_end = 0; 329 | 330 | return retval; 331 | } 332 | 333 | 334 | /////////////////////////////////////////////////////////////////////////////// 335 | int 336 | hokuyo::Laser::laserReadline(char *buf, int len, int timeout) 337 | { 338 | int current=0; 339 | 340 | struct pollfd ufd[1]; 341 | int retval; 342 | ufd[0].fd = laser_fd_; 343 | ufd[0].events = POLLIN; 344 | 345 | if (timeout == 0) 346 | timeout = -1; // For compatibility with former behavior, 0 means no timeout. For poll, negative means no timeout. 347 | 348 | while (true) 349 | { 350 | if (read_buf_start == read_buf_end) // Need to read? 351 | { 352 | if ((retval = poll(ufd, 1, timeout)) < 0) 353 | HOKUYO_EXCEPT(hokuyo::Exception, "poll failed -- error = %d: %s", errno, strerror(errno)); 354 | 355 | if (retval == 0) 356 | HOKUYO_EXCEPT(hokuyo::TimeoutException, "timeout reached"); 357 | 358 | if (ufd[0].revents & POLLERR) 359 | HOKUYO_EXCEPT(hokuyo::Exception, "error on socket, possibly unplugged"); 360 | 361 | int bytes = read(laser_fd_, read_buf, sizeof(read_buf)); 362 | if (bytes == -1 && errno != EAGAIN && errno != EWOULDBLOCK) 363 | HOKUYO_EXCEPT(hokuyo::Exception, "read failed"); 364 | read_buf_start = 0; 365 | read_buf_end = bytes; 366 | } 367 | 368 | while (read_buf_end != read_buf_start) 369 | { 370 | if (current == len - 1) 371 | { 372 | buf[current] = 0; 373 | HOKUYO_EXCEPT(hokuyo::Exception, "buffer filled without end of line being found"); 374 | } 375 | 376 | buf[current] = read_buf[read_buf_start++]; 377 | if (buf[current++] == '\n') 378 | { 379 | buf[current] = 0; 380 | return current; 381 | } 382 | } 383 | 384 | #ifdef USE_LOG_FILE 385 | long long outtime = timeHelper(); 386 | fprintf(logfile, "In: %lli.%09lli %s", outtime / 1000000000L, outtime % 1000000000L, buf); 387 | #endif 388 | } 389 | } 390 | 391 | 392 | char* 393 | hokuyo::Laser::laserReadlineAfter(char* buf, int len, const char* str, int timeout) 394 | { 395 | buf[0] = 0; 396 | char* ind = &buf[0]; 397 | 398 | int bytes_read = 0; 399 | int skipped = 0; 400 | 401 | while ((strncmp(buf, str, strlen(str))) != 0) { 402 | bytes_read = laserReadline(buf,len,timeout); 403 | 404 | if ((skipped += bytes_read) > MAX_SKIPPED) 405 | HOKUYO_EXCEPT(hokuyo::Exception, "too many bytes skipped while searching for match"); 406 | } 407 | 408 | return ind += strlen(str); 409 | } 410 | 411 | 412 | /////////////////////////////////////////////////////////////////////////////// 413 | void 414 | hokuyo::Laser::querySensorConfig() 415 | { 416 | if (!portOpen()) 417 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 418 | 419 | if (sendCmd("PP",1000) != 0) 420 | HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting configuration information"); 421 | 422 | char buf[100]; 423 | char* ind; 424 | 425 | ind = laserReadlineAfter(buf,100,"DMIN:",-1); 426 | sscanf(ind, "%d", &dmin_); 427 | 428 | ind = laserReadlineAfter(buf,100,"DMAX:",-1); 429 | sscanf(ind, "%d", &dmax_); 430 | 431 | ind = laserReadlineAfter(buf,100,"ARES:",-1); 432 | sscanf(ind, "%d", &ares_); 433 | 434 | ind = laserReadlineAfter(buf,100,"AMIN:",-1); 435 | sscanf(ind, "%d", &amin_); 436 | 437 | ind = laserReadlineAfter(buf,100,"AMAX:",-1); 438 | sscanf(ind, "%d", &amax_); 439 | 440 | ind = laserReadlineAfter(buf,100,"AFRT:",-1); 441 | sscanf(ind, "%d", &afrt_); 442 | 443 | ind = laserReadlineAfter(buf,100,"SCAN:",-1); 444 | sscanf(ind, "%d", &rate_); 445 | 446 | return; 447 | } 448 | 449 | 450 | /////////////////////////////////////////////////////////////////////////////// 451 | bool 452 | hokuyo::Laser::checkSum(const char* buf, int buf_len) 453 | { 454 | char sum = 0; 455 | for (int i = 0; i < buf_len - 2; i++) 456 | sum += (unsigned char)(buf[i]); 457 | 458 | if ((sum & 63) + 0x30 == buf[buf_len - 2]) 459 | return true; 460 | else 461 | return false; 462 | } 463 | 464 | 465 | /////////////////////////////////////////////////////////////////////////////// 466 | uint64_t 467 | hokuyo::Laser::readTime(int timeout) 468 | { 469 | char buf[100]; 470 | 471 | laserReadline(buf, 100, timeout); 472 | if (!checkSum(buf, 6)) 473 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on time stamp."); 474 | 475 | unsigned int laser_time = ((buf[0]-0x30) << 18) | ((buf[1]-0x30) << 12) | ((buf[2]-0x30) << 6) | (buf[3] - 0x30); 476 | 477 | if (laser_time == last_time_) 478 | { 479 | if (++time_repeat_count_ > 2) 480 | { 481 | HOKUYO_EXCEPT(hokuyo::RepeatedTimeException, "The timestamp has not changed for %d reads", time_repeat_count_); 482 | } 483 | else if (time_repeat_count_ > 0) 484 | ROS_DEBUG("The timestamp has not changed for %d reads. Ignoring for now.", time_repeat_count_); 485 | } 486 | else 487 | { 488 | time_repeat_count_ = 0; 489 | } 490 | 491 | if (laser_time < last_time_) 492 | wrapped_++; 493 | 494 | last_time_ = laser_time; 495 | 496 | return (uint64_t)((wrapped_ << 24) | laser_time)*(uint64_t)(1000000); 497 | } 498 | 499 | 500 | /////////////////////////////////////////////////////////////////////////////// 501 | void 502 | hokuyo::Laser::readData(hokuyo::LaserScan& scan, bool has_intensity, int timeout) 503 | { 504 | scan.ranges.clear(); 505 | scan.intensities.clear(); 506 | 507 | int data_size = 3; 508 | if (has_intensity) 509 | data_size = 6; 510 | 511 | char buf[100]; 512 | 513 | int ind = 0; 514 | 515 | scan.self_time_stamp = readTime(timeout); 516 | 517 | int bytes; 518 | 519 | int range; 520 | float intensity; 521 | 522 | for (;;) 523 | { 524 | bytes = laserReadline(&buf[ind], 100 - ind, timeout); 525 | 526 | if (bytes == 1) // This is \n\n so we should be done 527 | return; 528 | 529 | if (!checkSum(&buf[ind], bytes)) 530 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on data read."); 531 | 532 | bytes += ind - 2; 533 | 534 | // Read as many ranges as we can get 535 | if(dmax_ > 20000){ // Check error codes for the UTM 30LX (it is the only one with the long max range and has different error codes) 536 | for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) 537 | { 538 | if (scan.ranges.size() < MAX_READINGS) 539 | { 540 | range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); 541 | 542 | switch (range) // See the SCIP2.0 reference on page 12, Table 4 543 | { 544 | case 1: // No Object in Range 545 | scan.ranges.push_back(std::numeric_limits::infinity()); 546 | break; 547 | case 2: // Object is too near (Internal Error) 548 | scan.ranges.push_back(-std::numeric_limits::infinity()); 549 | break; 550 | case 3: // Measurement Error (May be due to interference) 551 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 552 | break; 553 | case 4: // Object out of range (at the near end) 554 | ///< @todo, Should this be an Infinity Instead? 555 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 556 | break; 557 | case 5: // Other errors 558 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 559 | break; 560 | default: 561 | scan.ranges.push_back(((float)range)/1000.0); 562 | } 563 | 564 | if (has_intensity) 565 | { 566 | intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); 567 | scan.intensities.push_back(intensity); 568 | } 569 | } 570 | else 571 | { 572 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected"); 573 | } 574 | } 575 | } else { // Check error codes for all other lasers (URG-04LX UBG-04LX-F01 UHG-08LX) 576 | for (int j = 0; j < bytes - (bytes % data_size); j+=data_size) 577 | { 578 | if (scan.ranges.size() < MAX_READINGS) 579 | { 580 | range = (((buf[j]-0x30) << 12) | ((buf[j+1]-0x30) << 6) | (buf[j+2]-0x30)); 581 | 582 | switch (range) // See the SCIP2.0 reference on page 12, Table 3 583 | { 584 | case 0: // Detected object is possibly at 22m 585 | scan.ranges.push_back(std::numeric_limits::infinity()); 586 | break; 587 | case 1: // Reflected light has low intensity 588 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 589 | break; 590 | case 2: // Reflected light has low intensity 591 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 592 | break; 593 | case 6: // Detected object is possibly at 5.7m 594 | scan.ranges.push_back(std::numeric_limits::infinity()); 595 | break; 596 | case 7: // Distance data on the preceding and succeeding steps have errors 597 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 598 | break; 599 | case 8: // Intensity difference of two waves 600 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 601 | break; 602 | case 9: // The same step had error in the last two scan 603 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 604 | break; 605 | case 10: // Others 606 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 607 | break; 608 | case 11: // Others 609 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 610 | break; 611 | case 12: // Others 612 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 613 | break; 614 | case 13: // Others 615 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 616 | break; 617 | case 14: // Others 618 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 619 | break; 620 | case 15: // Others 621 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 622 | break; 623 | case 16: // Others 624 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 625 | break; 626 | case 17: // Others 627 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 628 | break; 629 | case 18: // Error reading due to strong reflective object 630 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 631 | break; 632 | case 19: // Non-Measurable step 633 | scan.ranges.push_back(std::numeric_limits::quiet_NaN()); 634 | break; 635 | default: 636 | scan.ranges.push_back(((float)range)/1000.0); 637 | } 638 | 639 | if (has_intensity) 640 | { 641 | intensity = (((buf[j+3]-0x30) << 12) | ((buf[j+4]-0x30) << 6) | (buf[j+5]-0x30)); 642 | scan.intensities.push_back(intensity); 643 | } 644 | } 645 | else 646 | { 647 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Got more readings than expected"); 648 | } 649 | } 650 | } 651 | // Shuffle remaining bytes to front of buffer to get them on the next loop 652 | ind = 0; 653 | for (int j = bytes - (bytes % data_size); j < bytes ; j++) 654 | buf[ind++] = buf[j]; 655 | } 656 | } 657 | 658 | 659 | /////////////////////////////////////////////////////////////////////////////// 660 | int 661 | hokuyo::Laser::pollScan(hokuyo::LaserScan& scan, double min_ang, double max_ang, int cluster, int timeout) 662 | { 663 | if (!portOpen()) 664 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 665 | 666 | int status; 667 | 668 | // Always clear ranges/intensities so we can return easily in case of erro 669 | scan.ranges.clear(); 670 | scan.intensities.clear(); 671 | 672 | // clustering of 0 and 1 are actually the same 673 | if (cluster == 0) 674 | cluster = 1; 675 | 676 | int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI)); 677 | int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI)); 678 | 679 | char cmdbuf[MAX_CMD_LEN]; 680 | 681 | sprintf(cmdbuf,"GD%.4d%.4d%.2d", min_i, max_i, cluster); 682 | 683 | status = sendCmd(cmdbuf, timeout); 684 | 685 | scan.system_time_stamp = timeHelper() + offset_; 686 | 687 | if (status != 0) 688 | return status; 689 | 690 | // Populate configuration 691 | scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); 692 | scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); 693 | scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); 694 | scan.config.time_increment = (60.0)/(double)(rate_ * ares_); 695 | scan.config.scan_time = 0.0; 696 | scan.config.min_range = dmin_ / 1000.0; 697 | scan.config.max_range = dmax_ / 1000.0; 698 | 699 | readData(scan, false, timeout); 700 | 701 | long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); 702 | 703 | scan.system_time_stamp += inc; 704 | scan.self_time_stamp += inc; 705 | 706 | return 0; 707 | } 708 | 709 | int 710 | hokuyo::Laser::laserOn() { 711 | int res = sendCmd("BM",1000); 712 | if (res == 1) 713 | HOKUYO_EXCEPT(hokuyo::Exception, "Unable to control laser due to malfunction."); 714 | return res; 715 | } 716 | 717 | int 718 | hokuyo::Laser::laserOff() { 719 | return sendCmd("QT",1000); 720 | } 721 | 722 | int 723 | hokuyo::Laser::stopScanning() { 724 | try { 725 | return laserOff(); 726 | } 727 | catch (hokuyo::Exception &e) 728 | { 729 | // Ignore exception because we might have gotten part of a scan 730 | // instead of the expected response, which shows up as a bad checksum. 731 | laserFlush(); 732 | } 733 | return laserOff(); // This one should work because the scan is stopped. 734 | } 735 | 736 | /////////////////////////////////////////////////////////////////////////////// 737 | int 738 | hokuyo::Laser::requestScans(bool intensity, double min_ang, double max_ang, int cluster, int skip, int count, int timeout) 739 | { 740 | if (!portOpen()) 741 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 742 | 743 | //! @todo check that values are within range? 744 | 745 | int status; 746 | 747 | if (cluster == 0) 748 | cluster = 1; 749 | 750 | int min_i = (int)(afrt_ + min_ang*ares_/(2.0*M_PI)); 751 | int max_i = (int)(afrt_ + max_ang*ares_/(2.0*M_PI)); 752 | 753 | char cmdbuf[MAX_CMD_LEN]; 754 | 755 | char intensity_char = 'D'; 756 | if (intensity) 757 | intensity_char = 'E'; 758 | 759 | sprintf(cmdbuf,"M%c%.4d%.4d%.2d%.1d%.2d", intensity_char, min_i, max_i, cluster, skip, count); 760 | 761 | status = sendCmd(cmdbuf, timeout); 762 | 763 | return status; 764 | } 765 | 766 | bool 767 | hokuyo::Laser::isIntensitySupported() 768 | { 769 | hokuyo::LaserScan scan; 770 | 771 | if (!portOpen()) 772 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 773 | 774 | // Try an intensity command. 775 | try 776 | { 777 | requestScans(1, 0, 0, 0, 0, 1); 778 | serviceScan(scan, 1000); 779 | return true; 780 | } 781 | catch (hokuyo::Exception &e) 782 | {} 783 | 784 | // Try a non intensity command. 785 | try 786 | { 787 | requestScans(0, 0, 0, 0, 0, 1); 788 | serviceScan(scan, 1000); 789 | return false; 790 | } 791 | catch (hokuyo::Exception &e) 792 | { 793 | HOKUYO_EXCEPT(hokuyo::Exception, "Exception whil trying to determine if intensity scans are supported.") 794 | } 795 | } 796 | 797 | int 798 | hokuyo::Laser::serviceScan(hokuyo::LaserScan& scan, int timeout) 799 | { 800 | if (!portOpen()) 801 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 802 | 803 | // Always clear ranges/intensities so we can return easily in case of erro 804 | scan.ranges.clear(); 805 | scan.intensities.clear(); 806 | 807 | char buf[100]; 808 | 809 | bool intensity = false; 810 | int min_i; 811 | int max_i; 812 | int cluster; 813 | int skip; 814 | int left; 815 | 816 | char* ind; 817 | 818 | int status = -1; 819 | 820 | do { 821 | ind = laserReadlineAfter(buf, 100, "M",timeout); 822 | scan.system_time_stamp = timeHelper() + offset_; 823 | 824 | if (ind[0] == 'D') 825 | intensity = false; 826 | else if (ind[0] == 'E') 827 | intensity = true; 828 | else 829 | continue; 830 | 831 | ind++; 832 | 833 | sscanf(ind, "%4d%4d%2d%1d%2d", &min_i, &max_i, &cluster, &skip, &left); 834 | laserReadline(buf,100,timeout); 835 | 836 | buf[4] = 0; 837 | 838 | if (!checkSum(buf, 4)) 839 | HOKUYO_EXCEPT(hokuyo::CorruptedDataException, "Checksum failed on status code: %s", buf); 840 | 841 | sscanf(buf, "%2d", &status); 842 | 843 | if (status != 99) 844 | return status; 845 | 846 | } while(status != 99); 847 | 848 | scan.config.min_angle = (min_i - afrt_) * (2.0*M_PI)/(ares_); 849 | scan.config.max_angle = (max_i - afrt_) * (2.0*M_PI)/(ares_); 850 | scan.config.ang_increment = cluster*(2.0*M_PI)/(ares_); 851 | scan.config.time_increment = (60.0)/(double)(rate_ * ares_); 852 | scan.config.scan_time = (60.0 * (skip + 1))/((double)(rate_)); 853 | scan.config.min_range = dmin_ / 1000.0; 854 | scan.config.max_range = dmax_ / 1000.0; 855 | 856 | readData(scan, intensity, timeout); 857 | 858 | long long inc = (long long)(min_i * scan.config.time_increment * 1000000000); 859 | 860 | scan.system_time_stamp += inc; 861 | scan.self_time_stamp += inc; 862 | 863 | // printf("Scan took %lli.\n", -scan.system_time_stamp + timeHelper() + offset_); 864 | 865 | return 0; 866 | } 867 | 868 | ////////////////////////////////////////////////////////////////////////////// 869 | void 870 | hokuyo::Laser::queryVersionInformation() 871 | { 872 | if (!portOpen()) 873 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 874 | 875 | if (sendCmd("VV",1000) != 0) 876 | HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting version information"); 877 | 878 | char buf[100]; 879 | vendor_name_ = laserReadlineAfter(buf, 100, "VEND:"); 880 | vendor_name_ = vendor_name_.substr(0,vendor_name_.length() - 3); 881 | 882 | product_name_ = laserReadlineAfter(buf, 100, "PROD:"); 883 | product_name_ = product_name_.substr(0,product_name_.length() - 3); 884 | 885 | firmware_version_ = laserReadlineAfter(buf, 100, "FIRM:"); 886 | firmware_version_ = firmware_version_.substr(0,firmware_version_.length() - 3); 887 | 888 | protocol_version_ = laserReadlineAfter(buf, 100, "PROT:"); 889 | protocol_version_ = protocol_version_.substr(0,protocol_version_.length() - 3); 890 | 891 | // This crazy naming scheme is for backward compatibility. Initially 892 | // the serial number always started with an H. Then it got changed to a 893 | // zero. For a while the driver was removing the leading zero in the 894 | // serial number. This is fine as long as it is indeed a zero in front. 895 | // The current behavior is backward compatible but will accomodate full 896 | // length serial numbers. 897 | serial_number_ = laserReadlineAfter(buf, 100, "SERI:"); 898 | serial_number_ = serial_number_.substr(0,serial_number_.length() - 3); 899 | if (serial_number_[0] == '0') 900 | serial_number_[0] = 'H'; 901 | else if (serial_number_[0] != 'H') 902 | serial_number_ = 'H' + serial_number_; 903 | } 904 | 905 | 906 | ////////////////////////////////////////////////////////////////////////////// 907 | std::string 908 | hokuyo::Laser::getID() 909 | { 910 | if (!portOpen()) 911 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 912 | 913 | return serial_number_; 914 | } 915 | 916 | 917 | ////////////////////////////////////////////////////////////////////////////// 918 | std::string 919 | hokuyo::Laser::getFirmwareVersion() 920 | { 921 | if (!portOpen()) 922 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 923 | 924 | return firmware_version_; 925 | } 926 | 927 | 928 | ////////////////////////////////////////////////////////////////////////////// 929 | std::string 930 | hokuyo::Laser::getProtocolVersion() 931 | { 932 | if (!portOpen()) 933 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 934 | 935 | return protocol_version_; 936 | } 937 | 938 | 939 | ////////////////////////////////////////////////////////////////////////////// 940 | std::string 941 | hokuyo::Laser::getVendorName() 942 | { 943 | if (!portOpen()) 944 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 945 | 946 | return vendor_name_; 947 | } 948 | 949 | 950 | ////////////////////////////////////////////////////////////////////////////// 951 | std::string 952 | hokuyo::Laser::getProductName() 953 | { 954 | if (!portOpen()) 955 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 956 | 957 | return product_name_; 958 | } 959 | 960 | 961 | ////////////////////////////////////////////////////////////////////////////// 962 | std::string 963 | hokuyo::Laser::getStatus() 964 | { 965 | if (!portOpen()) 966 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 967 | 968 | if (sendCmd("II",1000) != 0) 969 | HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting device information information"); 970 | 971 | char buf[100]; 972 | char* stat = laserReadlineAfter(buf, 100, "STAT:"); 973 | 974 | std::string statstr(stat); 975 | statstr = statstr.substr(0,statstr.length() - 3); 976 | 977 | return statstr; 978 | } 979 | 980 | template 981 | C median(std::vector &v) 982 | { 983 | std::vector::iterator start = v.begin(); 984 | std::vector::iterator end = v.end(); 985 | std::vector::iterator median = start + (end - start) / 2; 986 | //std::vector::iterator quarter1 = median - (end - start) / 4; 987 | //std::vector::iterator quarter2 = median + (end - start) / 4; 988 | std::nth_element(start, median, end); 989 | //long long int medianval = *median; 990 | //std::nth_element(start, quarter1, end); 991 | //long long int quarter1val = *quarter1; 992 | //std::nth_element(start, quarter2, end); 993 | //long long int quarter2val = *quarter2; 994 | return *median; 995 | } 996 | 997 | #if 1 998 | long long int hokuyo::Laser::getHokuyoClockOffset(int reps, int timeout) 999 | { 1000 | std::vector offset(reps); 1001 | 1002 | sendCmd("TM0",timeout); 1003 | for (int i = 0; i < reps; i++) 1004 | { 1005 | long long int prestamp = timeHelper(); 1006 | sendCmd("TM1",timeout); 1007 | long long int hokuyostamp = readTime(); 1008 | long long int poststamp = timeHelper(); 1009 | offset[i] = hokuyostamp - (prestamp + poststamp) / 2; 1010 | //printf("%lli %lli %lli", hokuyostamp, prestamp, poststamp); 1011 | } 1012 | sendCmd("TM2",timeout); 1013 | 1014 | long long out = median(offset); 1015 | 1016 | return out; 1017 | } 1018 | 1019 | long long int hokuyo::Laser::getHokuyoScanStampToSystemStampOffset(bool intensity, double min_ang, double max_ang, int clustering, int skip, int reps, int timeout) 1020 | { 1021 | if (reps < 1) 1022 | reps = 1; 1023 | else if (reps > 99) 1024 | reps = 99; 1025 | 1026 | std::vector offset(reps); 1027 | 1028 | if (requestScans(intensity, min_ang, max_ang, clustering, skip, reps, timeout) != 0) 1029 | { 1030 | HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scan while caliblating time."); 1031 | return 1; 1032 | } 1033 | 1034 | hokuyo::LaserScan scan; 1035 | for (int i = 0; i < reps; i++) 1036 | { 1037 | serviceScan(scan, timeout); 1038 | //printf("%lli %lli\n", scan.self_time_stamp, scan.system_time_stamp); 1039 | offset[i] = scan.self_time_stamp - scan.system_time_stamp; 1040 | } 1041 | 1042 | return median(offset); 1043 | } 1044 | 1045 | ////////////////////////////////////////////////////////////////////////////// 1046 | long long 1047 | hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout) 1048 | { 1049 | offset_ = 0; 1050 | if (!portOpen()) 1051 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 1052 | 1053 | if (num <= 0) 1054 | num = 10; 1055 | 1056 | int ckreps = 1; 1057 | int scanreps = 1; 1058 | long long int start = getHokuyoClockOffset(ckreps, timeout); 1059 | long long int pre = 0; 1060 | std::vector samples(num); 1061 | for (int i = 0; i < num; i++) 1062 | { 1063 | long long int scan = getHokuyoScanStampToSystemStampOffset(intensity, min_ang, max_ang, clustering, skip, scanreps, timeout) - start; 1064 | long long int post = getHokuyoClockOffset(ckreps, timeout) - start; 1065 | samples[i] = scan - (post+pre)/2; 1066 | //printf("%lli %lli %lli %lli %lli\n", samples[i], post, pre, scan, pre - post); 1067 | //fflush(stdout); 1068 | pre = post; 1069 | } 1070 | 1071 | offset_ = median(samples); 1072 | //printf("%lli\n", median(samples)); 1073 | return offset_; 1074 | } 1075 | 1076 | #else 1077 | ////////////////////////////////////////////////////////////////////////////// 1078 | long long 1079 | hokuyo::Laser::calcLatency(bool intensity, double min_ang, double max_ang, int clustering, int skip, int num, int timeout) 1080 | { 1081 | ROS_DEBUG("Entering calcLatency."); 1082 | 1083 | if (!portOpen()) 1084 | HOKUYO_EXCEPT(hokuyo::Exception, "Port not open."); 1085 | 1086 | static const std::string buggy_version = "1.16.02(19/Jan./2010)"; 1087 | if (firmware_version_ == buggy_version) 1088 | { 1089 | ROS_INFO("Hokuyo firmware version %s detected. Using hard-coded time offset of -23 ms.", 1090 | buggy_version.c_str()); 1091 | offset_ = -23000000; 1092 | } 1093 | else 1094 | { 1095 | offset_ = 0; 1096 | 1097 | uint64_t comp_time = 0; 1098 | uint64_t laser_time = 0; 1099 | long long diff_time = 0; 1100 | long long drift_time = 0; 1101 | long long tmp_offset1 = 0; 1102 | long long tmp_offset2 = 0; 1103 | 1104 | int count = 0; 1105 | 1106 | sendCmd("TM0",timeout); 1107 | count = 100; 1108 | 1109 | for (int i = 0; i < count;i++) 1110 | { 1111 | usleep(1000); 1112 | sendCmd("TM1",timeout); 1113 | comp_time = timeHelper(); 1114 | try 1115 | { 1116 | laser_time = readTime(); 1117 | 1118 | diff_time = comp_time - laser_time; 1119 | 1120 | tmp_offset1 += diff_time / count; 1121 | } catch (hokuyo::RepeatedTimeException &e) 1122 | { 1123 | // We expect to get Repeated Time's when hammering on the time server 1124 | continue; 1125 | } 1126 | } 1127 | 1128 | uint64_t start_time = timeHelper(); 1129 | usleep(5000000); 1130 | sendCmd("TM1;a",timeout); 1131 | sendCmd("TM1;b",timeout); 1132 | comp_time = timeHelper(); 1133 | drift_time = comp_time - start_time; 1134 | laser_time = readTime() + tmp_offset1; 1135 | diff_time = comp_time - laser_time; 1136 | double drift_rate = double(diff_time) / double(drift_time); 1137 | 1138 | sendCmd("TM2",timeout); 1139 | 1140 | if (requestScans(intensity, min_ang, max_ang, clustering, skip, num, timeout) != 0) 1141 | HOKUYO_EXCEPT(hokuyo::Exception, "Error requesting scans during latency calculation"); 1142 | 1143 | hokuyo::LaserScan scan; 1144 | 1145 | count = 200; 1146 | for (int i = 0; i < count;i++) 1147 | { 1148 | try 1149 | { 1150 | serviceScan(scan, 1000); 1151 | } catch (hokuyo::CorruptedDataException &e) { 1152 | continue; 1153 | } 1154 | 1155 | comp_time = scan.system_time_stamp; 1156 | drift_time = comp_time - start_time; 1157 | laser_time = scan.self_time_stamp + tmp_offset1 + (long long)(drift_time*drift_rate); 1158 | diff_time = laser_time - comp_time; 1159 | 1160 | tmp_offset2 += diff_time / count; 1161 | } 1162 | 1163 | offset_ = tmp_offset2; 1164 | 1165 | stopScanning(); 1166 | } 1167 | 1168 | ROS_DEBUG("Leaving calcLatency."); 1169 | 1170 | return offset_; 1171 | } 1172 | #endif 1173 | --------------------------------------------------------------------------------