Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox: [272] gearbox/trunk
|
From: <gb...@us...> - 2008-07-10 00:15:53
|
Revision: 272
http://gearbox.svn.sourceforge.net/gearbox/?rev=272&view=rev
Author: gbiggs
Date: 2008-07-09 17:15:51 -0700 (Wed, 09 Jul 2008)
Log Message:
-----------
Accepted flexiport and hokuyo_aist. Retired urg_nz.
Modified Paths:
--------------
gearbox/trunk/cmake/internal/Setup.cmake
gearbox/trunk/src/CMakeLists.txt
gearbox/trunk/submitted/CMakeLists.txt
Added Paths:
-----------
gearbox/trunk/retired/CMakeLists.txt
gearbox/trunk/retired/urg_nz/
gearbox/trunk/retired/urg_nz/CMakeLists.txt
gearbox/trunk/retired/urg_nz/doc.dox
gearbox/trunk/retired/urg_nz/example.cmake.in
gearbox/trunk/retired/urg_nz/example.cpp
gearbox/trunk/retired/urg_nz/example.readme
gearbox/trunk/retired/urg_nz/urg_nz.cpp
gearbox/trunk/retired/urg_nz/urg_nz.h
gearbox/trunk/src/flexiport/
gearbox/trunk/src/flexiport/CMakeLists.txt
gearbox/trunk/src/flexiport/doc.dox
gearbox/trunk/src/flexiport/flexiport.cpp
gearbox/trunk/src/flexiport/flexiport.h
gearbox/trunk/src/flexiport/flexiport_config.h.in
gearbox/trunk/src/flexiport/flexiport_types.h
gearbox/trunk/src/flexiport/logfile.cpp
gearbox/trunk/src/flexiport/logfile.h
gearbox/trunk/src/flexiport/logreaderport.cpp
gearbox/trunk/src/flexiport/logreaderport.h
gearbox/trunk/src/flexiport/logwriterport.cpp
gearbox/trunk/src/flexiport/logwriterport.h
gearbox/trunk/src/flexiport/port.cpp
gearbox/trunk/src/flexiport/port.h
gearbox/trunk/src/flexiport/serialport.cpp
gearbox/trunk/src/flexiport/serialport.h
gearbox/trunk/src/flexiport/tcpport.cpp
gearbox/trunk/src/flexiport/tcpport.h
gearbox/trunk/src/flexiport/test/
gearbox/trunk/src/flexiport/timeout.cpp
gearbox/trunk/src/flexiport/timeout.h
gearbox/trunk/src/flexiport/utils/
gearbox/trunk/src/hokuyo_aist/
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/doc.dox
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/src/hokuyo_aist/test/
Removed Paths:
-------------
gearbox/trunk/retired/urg_nz/CMakeLists.txt
gearbox/trunk/retired/urg_nz/doc.dox
gearbox/trunk/retired/urg_nz/example.cmake.in
gearbox/trunk/retired/urg_nz/example.cpp
gearbox/trunk/retired/urg_nz/example.readme
gearbox/trunk/retired/urg_nz/urg_nz.cpp
gearbox/trunk/retired/urg_nz/urg_nz.h
gearbox/trunk/src/flexiport/CMakeLists.txt
gearbox/trunk/src/flexiport/doc.dox
gearbox/trunk/src/flexiport/flexiport.cpp
gearbox/trunk/src/flexiport/flexiport.h
gearbox/trunk/src/flexiport/flexiport_config.h.in
gearbox/trunk/src/flexiport/flexiport_types.h
gearbox/trunk/src/flexiport/logfile.cpp
gearbox/trunk/src/flexiport/logfile.h
gearbox/trunk/src/flexiport/logreaderport.cpp
gearbox/trunk/src/flexiport/logreaderport.h
gearbox/trunk/src/flexiport/logwriterport.cpp
gearbox/trunk/src/flexiport/logwriterport.h
gearbox/trunk/src/flexiport/port.cpp
gearbox/trunk/src/flexiport/port.h
gearbox/trunk/src/flexiport/serialport.cpp
gearbox/trunk/src/flexiport/serialport.h
gearbox/trunk/src/flexiport/tcpport.cpp
gearbox/trunk/src/flexiport/tcpport.h
gearbox/trunk/src/flexiport/test/
gearbox/trunk/src/flexiport/timeout.cpp
gearbox/trunk/src/flexiport/timeout.h
gearbox/trunk/src/flexiport/utils/
gearbox/trunk/src/hokuyo_aist/CMakeLists.txt
gearbox/trunk/src/hokuyo_aist/doc.dox
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
gearbox/trunk/src/hokuyo_aist/test/
gearbox/trunk/src/urg_nz/
gearbox/trunk/submitted/flexiport/
gearbox/trunk/submitted/hokuyo_aist/
Modified: gearbox/trunk/cmake/internal/Setup.cmake
===================================================================
--- gearbox/trunk/cmake/internal/Setup.cmake 2008-07-09 23:57:17 UTC (rev 271)
+++ gearbox/trunk/cmake/internal/Setup.cmake 2008-07-10 00:15:51 UTC (rev 272)
@@ -84,7 +84,7 @@
#
ADD_SUBDIRECTORY( src )
ADD_SUBDIRECTORY( submitted )
-# ADD_SUBDIRECTORY( retired )
+ADD_SUBDIRECTORY( retired )
# Some cmake and shell scripts need to be installed
ADD_SUBDIRECTORY( cmake )
Added: gearbox/trunk/retired/CMakeLists.txt
===================================================================
--- gearbox/trunk/retired/CMakeLists.txt (rev 0)
+++ gearbox/trunk/retired/CMakeLists.txt 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,15 @@
+# A place for libraries that have been retired from the source tree.
+
+OPTION ( GBX_BUILD_RETIRED "Build retired libraries" OFF )
+MARK_AS_ADVANCED ( FORCE GBX_BUILD_RETIRED )
+
+IF( GBX_BUILD_RETIRED )
+
+ MESSAGE( STATUS "== RETIRED ==" )
+
+ # When adding new directories, please maintain order of inter-dependencies.
+ # Otherwise, maintain alphabetical order.
+
+ ADD_SUBDIRECTORY( urg_nz )
+
+ENDIF( GBX_BUILD_RETIRED )
Copied: gearbox/trunk/retired/urg_nz (from rev 258, gearbox/trunk/src/urg_nz)
Deleted: gearbox/trunk/retired/urg_nz/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/urg_nz/CMakeLists.txt 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/CMakeLists.txt 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,23 +0,0 @@
-SET( lib_name urg_nz )
-set ( lib_desc "Hokuyo URG laser scanner driver" )
-GBX_ADD_LICENSE( GPL )
-
-SET( build TRUE )
-GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
-GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "Only Linux OS is supported" )
-
-IF( build )
-
- INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
-
- SET( hdrs urg_nz.h )
- SET( srcs urg_nz.cpp )
-
- GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
- GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" "" "" "" )
-
- GBX_ADD_HEADERS( ${lib_name} ${hdrs} )
-
- GBX_ADD_EXAMPLE( ${lib_name} example.cmake.in example.cmake example.cpp example.readme )
-
-ENDIF( build )
Copied: gearbox/trunk/retired/urg_nz/CMakeLists.txt (from rev 271, gearbox/trunk/src/urg_nz/CMakeLists.txt)
===================================================================
--- gearbox/trunk/retired/urg_nz/CMakeLists.txt (rev 0)
+++ gearbox/trunk/retired/urg_nz/CMakeLists.txt 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,23 @@
+SET( lib_name urg_nz )
+set ( lib_desc "Hokuyo URG laser scanner driver" )
+GBX_ADD_LICENSE( GPL )
+
+SET( build TRUE )
+GBX_REQUIRE_OPTION( build LIB ${lib_name} ON )
+GBX_REQUIRE_VAR( build LIB ${lib_name} GBX_OS_LINUX "Only Linux OS is supported" )
+
+IF( build )
+
+ INCLUDE( ${GBX_CMAKE_DIR}/UseBasicRules.cmake )
+
+ SET( hdrs urg_nz.h )
+ SET( srcs urg_nz.cpp )
+
+ GBX_ADD_LIBRARY( ${lib_name} SHARED ${srcs} )
+ GBX_ADD_PKGCONFIG( ${lib_name} ${lib_desc} "" "" "" "" )
+
+ GBX_ADD_HEADERS( ${lib_name} ${hdrs} )
+
+ GBX_ADD_EXAMPLE( ${lib_name} example.cmake.in example.cmake example.cpp example.readme )
+
+ENDIF( build )
Deleted: gearbox/trunk/retired/urg_nz/doc.dox
===================================================================
--- gearbox/trunk/src/urg_nz/doc.dox 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/doc.dox 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,50 +0,0 @@
-/*!
-
-@ingroup gbx_libs
-@ingroup gbx_hardware
-@ingroup gbx_c
-@ingroup gbx_linux
-@defgroup gbx_library_urg_nz liburg_nz
-@brief Hokuyo URG laser scanner driver.
-
-For a full list of functions and classes see @ref urg_nz.
-
-Header file:
-@verbatim
-#include <urglaser/urg_laser.h>
-@endverbatim
-
-@par Responsible Developer
- Geoffrey Biggs
-
-@par Copyright
- Toby Collett, Nico Blodow, Geoffrey Biggs
-
-@par License
- GPL
-
-@par Style guidelines
-
-- Naming conventions:
- - Class methods start with a capital letter.
- - Underscores in member, variable and type names.
- - \#define'd values in all capitals.
-- Formatting:
- - 4 space indentation.
- - Function declarations on one line.
- - Space between function name and arguments.
-- C++ API.
- - Functionality provided through classes with utility structures.
-- Units:
- - All internal units are in millimetres and radians.
-
-*/
-
-/*!
-@namespace urg_nz
-@brief URG laser scanner driver name space.
-
-This namespace is part of a library which provides a driver for the URG laser scanner driver.
-
-@see @ref gbx_library_urg_nz
-*/
Copied: gearbox/trunk/retired/urg_nz/doc.dox (from rev 271, gearbox/trunk/src/urg_nz/doc.dox)
===================================================================
--- gearbox/trunk/retired/urg_nz/doc.dox (rev 0)
+++ gearbox/trunk/retired/urg_nz/doc.dox 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,52 @@
+/*!
+
+@ingroup gbx_libs
+@ingroup gbx_hardware
+@ingroup gbx_c
+@ingroup gbx_linux
+@defgroup gbx_library_urg_nz liburg_nz
+@brief Hokuyo URG laser scanner driver.
+
+For a full list of functions and classes see @ref urg_nz.
+
+<b>This library has been retired.</b>
+
+Header file:
+@verbatim
+#include <urglaser/urg_laser.h>
+@endverbatim
+
+@par Responsible Developer
+ Geoffrey Biggs
+
+@par Copyright
+ Toby Collett, Nico Blodow, Geoffrey Biggs
+
+@par License
+ GPL
+
+@par Style guidelines
+
+- Naming conventions:
+ - Class methods start with a capital letter.
+ - Underscores in member, variable and type names.
+ - \#define'd values in all capitals.
+- Formatting:
+ - 4 space indentation.
+ - Function declarations on one line.
+ - Space between function name and arguments.
+- C++ API.
+ - Functionality provided through classes with utility structures.
+- Units:
+ - All internal units are in millimetres and radians.
+
+*/
+
+/*!
+@namespace urg_nz
+@brief URG laser scanner driver name space.
+
+This namespace is part of a library which provides a driver for the URG laser scanner driver.
+
+@see @ref gbx_library_urg_nz
+*/
Deleted: gearbox/trunk/retired/urg_nz/example.cmake.in
===================================================================
--- gearbox/trunk/src/urg_nz/example.cmake.in 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/example.cmake.in 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,10 +0,0 @@
-PROJECT( urg_nz_example )
-
-INCLUDE_DIRECTORIES( @CMAKE_INSTALL_PREFIX@ )
-
-ADD_EXECUTABLE( urg_nz_example example.cpp )
-TARGET_LINK_LIBRARIES( urg_nz_example urg_nz )
-SET_TARGET_PROPERTIES( urg_nz_example PROPERTIES
- LINK_FLAGS "-L@CMAKE_INSTALL_PREFIX@/lib/gearbox"
- INSTALL_RPATH "${INSTALL_RPATH};@CMAKE_INSTALL_PREFIX@/lib/gearbox"
- BUILD_WITH_INSTALL_RPATH TRUE )
Copied: gearbox/trunk/retired/urg_nz/example.cmake.in (from rev 271, gearbox/trunk/src/urg_nz/example.cmake.in)
===================================================================
--- gearbox/trunk/retired/urg_nz/example.cmake.in (rev 0)
+++ gearbox/trunk/retired/urg_nz/example.cmake.in 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,10 @@
+PROJECT( urg_nz_example )
+
+INCLUDE_DIRECTORIES( @CMAKE_INSTALL_PREFIX@ )
+
+ADD_EXECUTABLE( urg_nz_example example.cpp )
+TARGET_LINK_LIBRARIES( urg_nz_example urg_nz )
+SET_TARGET_PROPERTIES( urg_nz_example PROPERTIES
+ LINK_FLAGS "-L@CMAKE_INSTALL_PREFIX@/lib/gearbox"
+ INSTALL_RPATH "${INSTALL_RPATH};@CMAKE_INSTALL_PREFIX@/lib/gearbox"
+ BUILD_WITH_INSTALL_RPATH TRUE )
Deleted: gearbox/trunk/retired/urg_nz/example.cpp
===================================================================
--- gearbox/trunk/src/urg_nz/example.cpp 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/example.cpp 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,111 +0,0 @@
-#include <math.h>
-#include <stdio.h>
-#include <stdlib.h>
-#include <string.h>
-#include <unistd.h>
-
-#include <gearbox/urg_nz/urg_nz.h>
-
-int main (int argc, char **argv)
-{
- int opt, baud = 115200;
- bool useSerial = false;
- char port[256];
-
- // Get some options from the command line
- while ((opt = getopt (argc, argv, "p:b:s")) != -1)
- {
- switch (opt)
- {
- case 'p':
- strncpy (port, optarg, 256);
- break;
- case 'b':
- baud = atoi (optarg);
- if (baud != 19200 && baud != 57600 && baud != 115200)
- {
- printf ("Baud rate must be one of 19200, 57600 or 115200.\n");
- return 1;
- }
- break;
- case 's':
- useSerial = true;
- break;
- default:
- printf ("Usage: %s [-p port] [-b baud] [-s]\n\n"
- "-p port\tPort the laser scanner is connected to.\n"
- "-b baud\tBaud rate to connect at (19200, 57600 or 115200).\t"
- "-s\tUse RS232 connection instead of USB.\n", argv[0]);
- return 1;
- }
- }
-
- try
- {
- urg_nz::urg_laser laser; // Laser scanner object
- // Set the laser to verbose mode (so we see more information in the console)
- laser.SetVerbose (true);
-
- // Open the laser
- laser.Open (port, useSerial, baud);
-
- // Set the timeout to 1000ms
- laser.SetTimeOut (1000);
-
- // Check the SCIP protocol version
- int version = laser.GetSCIPVersion ();
- printf ("Laser is using SCIP protocol version %d\n", version);
-
- // Get the laser serial number
- int serial = 0;
- serial = laser.GetIDInfo ();
- printf ("Laser serial number:\t%d\n", serial);
-
- // Get the laser configuration
- urg_nz::urg_nz_laser_config_t config; // Laser configuration structure
- laser.GetSensorConfig (&config);
- printf ("Laser configuration:\n"
- "Min angle: %f\tMax angle: %f\tResolution: %f\tMax range: %f\n",
- config.min_angle, config.max_angle, config.resolution, config.max_range);
-
- // Calculate the minimum and maximum indices to retrieve - this is only necessary if you want
- // less than the full scan, otherwise simply don't supply min_i and max_i to urg_laser.GetReadings()
- // and the full scan will be returned.
- int minIndex = static_cast<int> (round ((urg_nz::MAX_READINGS / 2) + config.min_angle / config.resolution));
- int maxIndex = static_cast<int> (round ((urg_nz::MAX_READINGS / 2) + config.max_angle / config.resolution));
-
- if (useSerial && version == 1) // Baud rate changing is not currently supported on SCIP v2 scanners
- {
- // Change the baud rate
- if (laser.ChangeBaud (19200, 57600) == 0)
- printf ("Changed baud rate to 57600.\n");
- else
- printf ("Unable to change baud rate.\n");
- }
-
- // Get range readings from the laser
- urg_nz::urg_nz_laser_readings_t readings; // Laser readings structure
- printf ("Getting readings from %d to %d\n", minIndex, maxIndex);
- unsigned int numRead = laser.GetReadings (&readings, minIndex, maxIndex);
- printf ("Got %d range readings:\n", numRead);
- for (unsigned int ii = 0; ii < numRead; ii++)
- {
- float angle = config.min_angle + (ii * config.resolution);
- if (readings.Readings[ii] < 20) // Values less than 20 indicate no return in the scan
- printf ("%f: %f\t\t", angle, config.max_range);
- else
- printf ("%f: %d\t\t", angle, readings.Readings[ii]);
- }
- printf ("\n");
-
- // Close the laser
- laser.Close ();
- }
- catch (urg_nz::urg_nz_exception e)
- {
- printf ("Caught exception: (%d) %s\n", e.error_code, e.error_desc.c_str ());
- return -1;
- }
-
- return 0;
-}
Copied: gearbox/trunk/retired/urg_nz/example.cpp (from rev 271, gearbox/trunk/src/urg_nz/example.cpp)
===================================================================
--- gearbox/trunk/retired/urg_nz/example.cpp (rev 0)
+++ gearbox/trunk/retired/urg_nz/example.cpp 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,111 @@
+#include <math.h>
+#include <stdio.h>
+#include <stdlib.h>
+#include <string.h>
+#include <unistd.h>
+
+#include <gearbox/urg_nz/urg_nz.h>
+
+int main (int argc, char **argv)
+{
+ int opt, baud = 115200;
+ bool useSerial = false;
+ char port[256];
+
+ // Get some options from the command line
+ while ((opt = getopt (argc, argv, "p:b:s")) != -1)
+ {
+ switch (opt)
+ {
+ case 'p':
+ strncpy (port, optarg, 256);
+ break;
+ case 'b':
+ baud = atoi (optarg);
+ if (baud != 19200 && baud != 57600 && baud != 115200)
+ {
+ printf ("Baud rate must be one of 19200, 57600 or 115200.\n");
+ return 1;
+ }
+ break;
+ case 's':
+ useSerial = true;
+ break;
+ default:
+ printf ("Usage: %s [-p port] [-b baud] [-s]\n\n"
+ "-p port\tPort the laser scanner is connected to.\n"
+ "-b baud\tBaud rate to connect at (19200, 57600 or 115200).\t"
+ "-s\tUse RS232 connection instead of USB.\n", argv[0]);
+ return 1;
+ }
+ }
+
+ try
+ {
+ urg_nz::urg_laser laser; // Laser scanner object
+ // Set the laser to verbose mode (so we see more information in the console)
+ laser.SetVerbose (true);
+
+ // Open the laser
+ laser.Open (port, useSerial, baud);
+
+ // Set the timeout to 1000ms
+ laser.SetTimeOut (1000);
+
+ // Check the SCIP protocol version
+ int version = laser.GetSCIPVersion ();
+ printf ("Laser is using SCIP protocol version %d\n", version);
+
+ // Get the laser serial number
+ int serial = 0;
+ serial = laser.GetIDInfo ();
+ printf ("Laser serial number:\t%d\n", serial);
+
+ // Get the laser configuration
+ urg_nz::urg_nz_laser_config_t config; // Laser configuration structure
+ laser.GetSensorConfig (&config);
+ printf ("Laser configuration:\n"
+ "Min angle: %f\tMax angle: %f\tResolution: %f\tMax range: %f\n",
+ config.min_angle, config.max_angle, config.resolution, config.max_range);
+
+ // Calculate the minimum and maximum indices to retrieve - this is only necessary if you want
+ // less than the full scan, otherwise simply don't supply min_i and max_i to urg_laser.GetReadings()
+ // and the full scan will be returned.
+ int minIndex = static_cast<int> (round ((urg_nz::MAX_READINGS / 2) + config.min_angle / config.resolution));
+ int maxIndex = static_cast<int> (round ((urg_nz::MAX_READINGS / 2) + config.max_angle / config.resolution));
+
+ if (useSerial && version == 1) // Baud rate changing is not currently supported on SCIP v2 scanners
+ {
+ // Change the baud rate
+ if (laser.ChangeBaud (19200, 57600) == 0)
+ printf ("Changed baud rate to 57600.\n");
+ else
+ printf ("Unable to change baud rate.\n");
+ }
+
+ // Get range readings from the laser
+ urg_nz::urg_nz_laser_readings_t readings; // Laser readings structure
+ printf ("Getting readings from %d to %d\n", minIndex, maxIndex);
+ unsigned int numRead = laser.GetReadings (&readings, minIndex, maxIndex);
+ printf ("Got %d range readings:\n", numRead);
+ for (unsigned int ii = 0; ii < numRead; ii++)
+ {
+ float angle = config.min_angle + (ii * config.resolution);
+ if (readings.Readings[ii] < 20) // Values less than 20 indicate no return in the scan
+ printf ("%f: %f\t\t", angle, config.max_range);
+ else
+ printf ("%f: %d\t\t", angle, readings.Readings[ii]);
+ }
+ printf ("\n");
+
+ // Close the laser
+ laser.Close ();
+ }
+ catch (urg_nz::urg_nz_exception e)
+ {
+ printf ("Caught exception: (%d) %s\n", e.error_code, e.error_desc.c_str ());
+ return -1;
+ }
+
+ return 0;
+}
Deleted: gearbox/trunk/retired/urg_nz/example.readme
===================================================================
--- gearbox/trunk/src/urg_nz/example.readme 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/example.readme 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,24 +0,0 @@
-Building
---------
-
-The example can be built by making a directory (anywhere on your system where
-you have write permissions will do), changing to that directory and executing
-CMake with the example's source directory as an argument. For example, if you
-have installed GearBox into /usr/local, you could do the following:
-
-$ cd ~
-$ mkdir urg_nz_example
-$ cd urg_nz_example
-$ ccmake /usr/local/share/gearbox/urg_nz/
-
-Running
--------
-
-The example requires that you specify a path to the laser scanner's port and,
-optionally, a baud rate and whether to use an RS232 connection or not. For
-example:
-
-./urg_nz_example -p /dev/ttyS1 -b 115200 -s
-
-This will start the example, looking for the laser on port /dev/ttyS1 and
-using a baud rate of 115200bps in RS232 mode.
Copied: gearbox/trunk/retired/urg_nz/example.readme (from rev 271, gearbox/trunk/src/urg_nz/example.readme)
===================================================================
--- gearbox/trunk/retired/urg_nz/example.readme (rev 0)
+++ gearbox/trunk/retired/urg_nz/example.readme 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,24 @@
+Building
+--------
+
+The example can be built by making a directory (anywhere on your system where
+you have write permissions will do), changing to that directory and executing
+CMake with the example's source directory as an argument. For example, if you
+have installed GearBox into /usr/local, you could do the following:
+
+$ cd ~
+$ mkdir urg_nz_example
+$ cd urg_nz_example
+$ ccmake /usr/local/share/gearbox/urg_nz/
+
+Running
+-------
+
+The example requires that you specify a path to the laser scanner's port and,
+optionally, a baud rate and whether to use an RS232 connection or not. For
+example:
+
+./urg_nz_example -p /dev/ttyS1 -b 115200 -s
+
+This will start the example, looking for the laser on port /dev/ttyS1 and
+using a baud rate of 115200bps in RS232 mode.
Deleted: gearbox/trunk/retired/urg_nz/urg_nz.cpp
===================================================================
--- gearbox/trunk/src/urg_nz/urg_nz.cpp 2008-07-03 07:52:47 UTC (rev 258)
+++ gearbox/trunk/retired/urg_nz/urg_nz.cpp 2008-07-10 00:15:51 UTC (rev 272)
@@ -1,991 +0,0 @@
-/*
- * Hokuyo URG laser scanner driver class.
- * Originally written by Toby Collett for the Player project.
- */
-
-#include <assert.h>
-#include <stdio.h>
-#include <string.h>
-#include <unistd.h>
-#include <errno.h>
-#include <stdlib.h>
-#include <fcntl.h>
-#include <termios.h>
-#include <poll.h>
-#include <sstream>
-
-#include "urg_nz.h"
-
-using namespace urg_nz;
-using namespace std;
-
-#ifndef M_PI
- #define M_PI 3.14159265358979323846
-#endif
-// Convert radians to degrees
-#ifndef RTOD
- #define RTOD(r) ((r) * 180 / M_PI)
-#endif
-// Convert degrees to radians
-#ifndef DTOR
- #define DTOR(d) ((d) * M_PI / 180)
-#endif
-
-///////////////////////////////////////////////////////////////////////////////
-// Constructor
-///////////////////////////////////////////////////////////////////////////////
-urg_laser::urg_laser (void)
-{
- // Defaults to SCIP version 1
- SCIP_Version = 1;
- laser_port = NULL;
- verbose = false;
- poll_timeout = -1;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Destructor
-///////////////////////////////////////////////////////////////////////////////
-urg_laser::~urg_laser (void)
-{
- if (PortOpen ())
- Close ();
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Read functions
-///////////////////////////////////////////////////////////////////////////////
-int urg_laser::ReadUntil_nthOccurence (int file, int n, char c)
-{
- int retval = 0, bytes_read = 0;
- unsigned char buffer[2];
- buffer[0] = 0;
- buffer[1] = 0;
- for (int i = 0; i < n; i++)
- {
- do
- {
- retval = ReadUntil (file, &buffer[0], 1, poll_timeout);
- if (retval > 0)
- bytes_read += retval;
- } while (buffer[0] != c && retval > 0);
- }
- return bytes_read;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-int urg_laser::ReadUntil (int fd, unsigned char *buf, int len, int timeout)
-{
- int ret;
- int current=0;
- struct pollfd ufd[1];
- int retval;
-
- ufd[0].fd = fd;
- ufd[0].events = POLLIN;
-
- do
- {
- if(timeout >= 0)
- {
- if ((retval = poll (ufd, 1, timeout)) < 0)
- {
- stringstream error_desc;
- error_desc << "Error in ReadUntil at poll(): " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_READPOLL, error_desc.str ());
- }
- else if (retval == 0)
- {
- stringstream error_desc;
- error_desc << "Timed out on poll in ReadUntil";
- throw urg_nz_exception (URG_ERR_READTIMEOUT, error_desc.str ());
- }
- }
-
- ret = read (fd, &buf[current], len-current);
- if (ret < 0)
- {
- stringstream error_desc;
- error_desc << "Error in ReadUntil at read(): " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_READ, error_desc.str ());
- }
-
- current += ret;
- if (current > 2 && current < len && buf[current-2] == '\n' && buf[current-1] == '\n')
- {
- stringstream error_desc;
- error_desc << "Got an end of command while waiting for more data.";
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- } while (current < len);
- return current;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Port control functions
-///////////////////////////////////////////////////////////////////////////////
-int urg_laser::ChangeBaud (int curr_baud, int new_baud)
-{
- struct termios newtio;
- int fd;
- fd = fileno (laser_port);
- int new_baud_constant;
-
- switch (curr_baud)
- {
- case 19200:
- curr_baud = B19200;
- break;
- case 57600:
- curr_baud = B57600;
- break;
- case 115200:
- curr_baud = B115200;
- break;
- default:
- stringstream error_desc;
- error_desc << "Unknown current baud rate: " << curr_baud;
- throw urg_nz_exception (URG_ERR_BADBAUDRATE, error_desc.str ());
- }
-
- switch (new_baud)
- {
- case 19200:
- new_baud_constant = B19200;
- break;
- case 57600:
- new_baud_constant = B57600;
- break;
- case 115200:
- new_baud_constant = B115200;
- break;
- default:
- stringstream error_desc;
- error_desc << "Unknown new baud rate: " << new_baud;
- throw urg_nz_exception (URG_ERR_BADBAUDRATE, error_desc.str ());
- }
-
- if (tcgetattr (fd, &newtio) < 0)
- {
- close (fd);
- stringstream error_desc;
- error_desc << "Error in ChangeBaud at tcgetattr: " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_CHANGEBAUD, error_desc.str ());
- }
-
- cfmakeraw (&newtio);
- cfsetispeed (&newtio, curr_baud);
- cfsetospeed (&newtio, curr_baud);
-
- if (tcsetattr (fd, TCSAFLUSH, &newtio) < 0 )
- {
- close (fd);
- stringstream error_desc;
- error_desc << "Error in ChangeBaud at tcsetattr: " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_CHANGEBAUD, error_desc.str ());
- }
-
- unsigned char buf[17];
- memset (buf,0,sizeof (buf));
-
- // TODO: Check if this works with SCIP2.0
- if (SCIP_Version == 1)
- {
- buf[0] = 'S';
- switch (new_baud_constant)
- {
- case B19200:
- buf[1] = '0';
- buf[2] = '1';
- buf[3] = '9';
- buf[4] = '2';
- buf[5] = '0';
- buf[6] = '0';
- break;
- case B57600:
- buf[1] = '0';
- buf[2] = '5';
- buf[3] = '7';
- buf[4] = '6';
- buf[5] = '0';
- buf[6] = '0';
- break;
- case B115200:
- buf[1] = '1';
- buf[2] = '1';
- buf[3] = '5';
- buf[4] = '2';
- buf[5] = '0';
- buf[6] = '0';
- break;
- default:
- // Already checked above
- break;
- }
- buf[7] = '0';
- buf[8] = '0';
- buf[9] = '0';
- buf[10] = '0';
- buf[11] = '0';
- buf[12] = '0';
- buf[13] = '0';
- buf[14] = '\n';
- }
- else // SCIP 2
- {
- buf[0] = 'S';
- buf[1] = 'S';
- switch (new_baud)
- {
- case B19200:
- buf[2] = '0';
- buf[3] = '1';
- buf[4] = '9';
- buf[5] = '2';
- buf[6] = '0';
- buf[7] = '0';
- break;
- case B57600:
- buf[2] = '0';
- buf[3] = '5';
- buf[4] = '7';
- buf[5] = '6';
- buf[6] = '0';
- buf[7] = '0';
- break;
- case B115200:
- buf[2] = '1';
- buf[3] = '1';
- buf[4] = '5';
- buf[5] = '2';
- buf[6] = '0';
- buf[7] = '0';
- break;
- default:
- // Already checked above
- break;
- }
- buf[8] = '\n';
- }
-
- fprintf (laser_port, "%s", buf);
- memset (buf, 0, sizeof (buf));
- int len;
- // The docs say that the response ends in 'status LF LF', where
- // status is '0' if everything went alright. But it seems that
- // the response actually ends in 'LF status LF'.
- if (((len = ReadUntil (fd, buf, sizeof (buf), poll_timeout)) < 0) ||
- (buf[15] != '0'))
- {
- if (verbose)
- fprintf (stderr, "urg_nz: W: Failed to change baud rate to %d\n", new_baud);
- return -1;
- }
- else
- {
- if (tcgetattr (fd, &newtio) < 0)
- {
- close (fd);
- stringstream error_desc;
- error_desc << "Error in ChangeBaud at tcgetattr: " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_CHANGEBAUD, error_desc.str ());
- }
- cfmakeraw (&newtio);
- cfsetispeed (&newtio, new_baud_constant);
- cfsetospeed (&newtio, new_baud_constant);
- if (tcsetattr (fd, TCSAFLUSH, &newtio) < 0 )
- {
- close (fd);
- stringstream error_desc;
- error_desc << "Error in ChangeBaud at tcsetattr: " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_CHANGEBAUD, error_desc.str ());
- }
- else
- {
- usleep (200000);
- }
- }
- return 0;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-void urg_laser::Open (const char *port_name, bool use_serial, int baud)
-{
- if (PortOpen ())
- this->Close ();
-
- laser_port = fopen (port_name, "r+");
- if (laser_port == NULL)
- {
- stringstream error_desc;
- error_desc << "Failed to open port " << port_name << " << with error " << errno << ":" << strerror (errno);
- throw urg_nz_exception (URG_ERR_OPEN_FAILED, error_desc.str ());
- }
-
- int fd = fileno (laser_port);
- if (use_serial)
- {
- if (verbose)
- {
- fprintf (stderr, "urg_nz: I: Connecting using serial connection to %s\n", port_name);
- fprintf (stderr, "urg_nz: I: Trying to connect at 19200\n");
- }
- if (this->ChangeBaud (19200, baud) != 0)
- {
- if (verbose)
- fprintf (stderr, "urg_nz: I: Trying to connect at 57600\n");
- if (this->ChangeBaud (57600, baud) != 0)
- {
- if (verbose)
- fprintf (stderr, "urg_nz: I: Trying to connect at 115200\n");
- if (this->ChangeBaud (115200, baud) != 0)
- {
- close (fd);
- stringstream error_desc;
- error_desc << "Failed to connect at any baud";
- throw urg_nz_exception (URG_ERR_CONNECT_FAILED, error_desc.str ());
- }
- }
- }
- if (verbose)
- fprintf (stderr, "urg_nz: I: Successfully changed baud rate\n");
- }
- else
- {
- if (verbose)
- fprintf (stderr, "urg_nz: I: Connecting using USB connection to %s\n", port_name);
- // set up new settings
- struct termios newtio;
- memset (&newtio, 0, sizeof (newtio));
- newtio.c_cflag = /*(rate & CBAUD) |*/ CS8 | CLOCAL | CREAD;
- newtio.c_iflag = IGNPAR;
- newtio.c_oflag = 0;
- newtio.c_lflag = ICANON;
-
- // activate new settings
- tcflush (fd, TCIFLUSH);
- tcsetattr (fd, TCSANOW, &newtio);
- usleep (200000);
- GetSCIPVersion ();
- tcflush (fd, TCIOFLUSH);
- }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-void urg_laser::Close (void)
-{
- assert (this->laser_port);
-
- if (verbose)
- fprintf (stderr, "urg_nz: I: Closing port\n");
-
- tcflush (fileno (this->laser_port), TCIOFLUSH);
- if (fclose (this->laser_port) != 0)
- {
- stringstream error_desc;
- error_desc << "Error closing port: " << errno << ":" << strerror (errno);
- this->laser_port = NULL;
- throw urg_nz_exception (URG_ERR_CLOSE_FAILED, error_desc.str ());
- }
- this->laser_port = NULL;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-bool urg_laser::PortOpen (void)
-{
- return laser_port != NULL;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-// Laser sensor access functions
-///////////////////////////////////////////////////////////////////////////////
-unsigned int urg_laser::GetReadings (urg_nz_laser_readings_t *readings, unsigned int min_i, unsigned int max_i)
-{
- unsigned char buffer[16];
- unsigned int num_readings_read = 0;
-
- if (readings == NULL)
- {
- stringstream error_desc;
- error_desc << "NULL destination buffer";
- throw urg_nz_exception (URG_ERR_NODESTINATION, error_desc.str ());
- }
-
- if (!PortOpen ())
- {
- stringstream error_desc;
- error_desc << "Laser port is not open";
- throw urg_nz_exception (URG_ERR_NOPORT, error_desc.str ());
- }
-
- if (SCIP_Version == 1)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- // send the command
- fprintf (laser_port, "G00076801\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer, 10, poll_timeout);
-
- if (strncmp ((const char *) buffer, "G00076801", 9) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // check the returned status
- ReadUntil (file, buffer, 2, poll_timeout);
-
- if (buffer[0] != '0')
- {
- stringstream error_desc;
- error_desc << "Error reported by laser scanner: " << buffer[0] - '0';
- throw urg_nz_exception (URG_ERR_LASERERROR, error_desc.str ());
- }
-
- for (unsigned int i=0; ; ++i)
- {
- ReadUntil (file, buffer, 2, poll_timeout);
-
- if (buffer[0] == '\n' && buffer[1] == '\n')
- break;
- else if (buffer[0] == '\n')
- {
- buffer[0] = buffer[1];
- ReadUntil (file, &buffer[1], 1, poll_timeout);
- }
-
- if (i < MAX_READINGS)
- {
- readings->Readings[i] = ((buffer[0]-0x30) << 6) | (buffer[1]-0x30);
- num_readings_read++;
- }
- else
- {
- stringstream error_desc;
- error_desc << "Got too many readings: " << i;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- }
-
- // Shift the range readings down by min_i if necessary
- if (min_i > 0)
- {
- memmove (&readings->Readings[0], &readings->Readings[min_i], (max_i - min_i + 1) * sizeof (readings->Readings[0]));
- // Don't forget to adjust the number of readings to account for this
- num_readings_read -= (MAX_READINGS - max_i) + min_i - 1; // -1 because max_i is inclusive
- }
- }
- else // SCIP_Version == 2
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- // send the command
- fprintf (laser_port, "GD0000076801\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer, 13, poll_timeout);
-
- if (strncmp ((const char *) buffer, "GD0000076801", 12) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // check the returned status
- ReadUntil (file, buffer, 3, poll_timeout);
- buffer[2] = 0;
- if (buffer[0] != '0' || buffer[1] != '0')
- {
- stringstream error_desc;
- error_desc << "Error reported by laser scanner: " << (buffer[0] - '0') * 10 + (buffer[1] - '0');
- throw urg_nz_exception (URG_ERR_LASERERROR, error_desc.str ());
- }
-
- ReadUntil_nthOccurence (file, 2, (char)0xa);
-
- // NOTE: This only works for 769 requested samples.. (64 data bytes
- // blocks are not the best choice for 3-byte values...)
-
- for (unsigned int i = 0; ; ++i)
- {
- ReadUntil (file, buffer, 3, poll_timeout);
-
- if ((buffer[1] == '\n') && (buffer[2] == '\n'))
- break;
- else if (buffer[2] == '\n')
- ReadUntil (file, &buffer[1], 2, poll_timeout);
- else if (buffer[0] == '\n')
- {
- if (i <= MAX_READINGS)
- {
- readings->Readings[i - 1] = ((readings->Readings[i - 1] & 0xFFC0) | (buffer[1]-0x30));
- buffer [0] = buffer [2];
- ReadUntil (file, &buffer[1], 2, poll_timeout);
- }
- else
- {
- stringstream error_desc;
- error_desc << "Got too many readings: " << i;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- }
- else if (buffer[1] == '\n')
- {
- buffer[0] = buffer[2];
- ReadUntil (file, &buffer[1], 2, poll_timeout);
- }
-
- if (i < MAX_READINGS)
- {
- readings->Readings[i] = ((buffer[0]-0x30) << 12) | ((buffer[1]-0x30) << 6) | (buffer[2]-0x30);
- num_readings_read++;
- if ((readings->Readings[i] > 5600) && (i >= min_i) && (i <= max_i))
- {
- stringstream error_desc;
- error_desc << "Reading " << i << " value of " << readings->Readings[i] << " is bigger than 5.6 metres.";
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- }
- else
- {
- stringstream error_desc;
- error_desc << "Got too many readings: " << i;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- }
-
- // Shift the range readings down by min_i if necessary
- if (min_i > 0)
- {
- memmove (&readings->Readings[0], &readings->Readings[min_i], (max_i - min_i + 1) * sizeof (readings->Readings[0]));
- // Don't forget to adjust the number of readings to account for this
- num_readings_read -= (MAX_READINGS - max_i) + min_i - 1; // -1 because max_i is inclusive
- }
- }
-
- return num_readings_read;
-}
-
-//////////////////////////////////////////////////////////////////////////////
-int urg_laser::GetIDInfo (void)
-{
- unsigned char buffer [18];
- memset (buffer, 0, 18);
- int i;
- int id;
-
- if (!PortOpen ())
- {
- stringstream error_desc;
- error_desc << "Laser port is not open";
- throw urg_nz_exception (URG_ERR_NOPORT, error_desc.str ());
- }
-
- tcflush (fileno (laser_port), TCIFLUSH);
-
- if (SCIP_Version == 1)
- {
- // send the command
- fprintf (laser_port, "V\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer, 2, poll_timeout);
-
- if (strncmp ((const char *) buffer, "V", 1) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // check the returned status
- ReadUntil (file, buffer, 2, poll_timeout);
-
- if (buffer[0] != '0')
- {
- stringstream error_desc;
- error_desc << "Error reported by laser scanner: " << buffer[0] - '0';
- throw urg_nz_exception (URG_ERR_LASERERROR, error_desc.str ());
- }
-
- buffer[0] = 0;
- // Read the rest of the values
- for (i = 0; i < 4; i++)
- {
- do
- {
- ReadUntil (file, &buffer[0], 1, poll_timeout);
- } while (buffer[0] != 0xa);
- }
-
- // Read "SERI:H"
- ReadUntil (file, buffer, 6, poll_timeout);
- // Read the serial number value
- for (i = 0; ; i++)
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- if (buffer[i] == 0xa)
- break;
- }
-
- id = atol ((const char*)buffer);
- // Read the last LF
- ReadUntil (file, buffer, 1, poll_timeout);
- }
- else // SCIP_Version == 2
- {
- // send the command
- fprintf (laser_port, "VV\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer, 7, poll_timeout);
-
- if (strncmp ((const char *) buffer, "VV\n00P\n", 7) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- buffer[0] = 0;
- // Read the rest of the values
- for (i = 0; i < 4; i++)
- {
- do
- {
- ReadUntil (file, &buffer[0], 1, poll_timeout);
- } while (buffer[0] != 0xa);
- }
-
- // Read "SERI:H"
- ReadUntil (file, buffer, 6, poll_timeout);
- // Read the serial number value
- for (i = 0; ; i++)
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- if (buffer[i] == ';')
- {
- buffer[i] = 0;
- break;
- }
- }
-
- id = atol ((const char*)buffer);
-
- ReadUntil (file, buffer, 3, poll_timeout);
- }
-
- return id;
-}
-
-///////////////////////////////////////////////////////////////////////////////
-void urg_laser::GetSensorConfig (urg_nz_laser_config_t *cfg)
-{
- memset (cfg, 0, sizeof (urg_nz_laser_config_t));
-
- if (SCIP_Version == 1)
- {
- unsigned char buffer[10];
- memset (buffer, 0, 10);
- tcflush (fileno (laser_port), TCIFLUSH);
- // send the command
- fprintf (laser_port, "V\n");
-
- // Set the default resolution
- cfg->resolution = DTOR (270.0) / static_cast<float> (MAX_READINGS);
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer, 4, poll_timeout);
-
- if (strncmp ((const char *) buffer, "V\n0\n", 4) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // The following might not work on all versions of the hokuyos
- // since it reads out the Product description returned by 'V'
-
- ReadUntil_nthOccurence (file, 2, (char)0xa);
-
- // Read FIRM:
- ReadUntil (file, buffer, 5, poll_timeout);
-
- if (strncmp ((const char *) buffer, "FIRM:", 5) == 0)
- {
- // Read the firmware version major value
- ReadUntil (file, buffer, 5, poll_timeout);
- buffer[1] = 0;
- int firmware = atol ((const char*)buffer);
- if (verbose)
- fprintf (stderr, "urg_nz: I: Firmware major version is %d\n", firmware);
-
- if (firmware < 3)
- {
- ReadUntil_nthOccurence (file, 4, (char)0xa);
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Bad firmware version: " << firmware;
- throw urg_nz_exception (URG_ERR_BADFIRMWARE, error_desc.str ());
- }
- }
-
- ReadUntil_nthOccurence(file, 1, (char)'(');
- ReadUntil_nthOccurence(file, 1, (char)'-');
-
- int i = 0;
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- } while (buffer[i++] != '[');
-
- buffer[i-1] = 0;
- int max_range = atol((const char*)buffer);
-
- ReadUntil_nthOccurence (file, 2, (char)',');
- i = 0;
- do
- {
- ReadUntil(file, &buffer[i], 1, poll_timeout);
- } while (buffer[i++] != '-');
-
- buffer[i-1] = 0;
- int min_i = atol ((const char*)buffer);
- i = 0;
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- } while (buffer[i++] != '[');
- buffer[i-1] = 0;
-
- int max_i = atol ((const char*)buffer);
-
- ReadUntil (file, buffer, 4, poll_timeout);
- if (strncmp ((const char *) buffer, "step", 4) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading angle_min_idx and angle_max_idx. Check firmware version.";
- throw urg_nz_exception (URG_ERR_READ, error_desc.str ());
- }
- cfg->max_range = max_range;
- cfg->min_angle = (min_i-384)*cfg->resolution;
- cfg->max_angle = (max_i-384)*cfg->resolution;
- if (verbose)
- {
- fprintf (stderr, "urg_nz: I: URG-04 specifications: [min_angle, max_angle, resolution, max_range] = [%f, %f, %f, %f]\n",
- cfg->min_angle, cfg->max_angle, cfg->resolution, cfg->max_range);
- }
- tcflush (fileno(laser_port), TCIFLUSH);
- }
- else // SCIP_Version = 2
- {
- // ask hokuyo: PP
- unsigned char buffer[10];
- memset (buffer, 0, 10);
- tcflush (fileno (laser_port), TCIFLUSH);
-
- // send the command
- fprintf (laser_port, "PP\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- ReadUntil (file, buffer,7, poll_timeout);
-
- if (strncmp ((const char *) buffer, "PP\n00P\n", 7) != 0)
- {
- tcflush (fileno (laser_port), TCIFLUSH);
- stringstream error_desc;
- error_desc << "Error reading command result: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
- int i = 0;
- ReadUntil_nthOccurence (file, 2, (char)0xa);
- // read DMAX
- ReadUntil_nthOccurence (file, 1, ':');
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- i++;
- } while (buffer[i-1] != ';');
- buffer[i-1] = 0;
- cfg->max_range = atol ((const char*)buffer);
-
- // read angular resolution
- ReadUntil_nthOccurence (file, 1, ':');
- i = 0;
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- i++;
- } while (buffer[i-1] != ';');
- buffer[i-1] = 0;
- cfg->resolution = DTOR (360.0 / atol ((const char*)buffer));
-
- // read AMIN
- ReadUntil_nthOccurence (file, 1, ':');
- i = 0;
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- i++;
- } while (buffer[i-1] != ';');
- buffer[i-1] = 0;
- cfg->min_angle = atol ((const char*)buffer);
- cfg->min_angle -= 384.0;
- cfg->min_angle *= cfg->resolution;
-
- // read AMAX
- ReadUntil_nthOccurence (file, 1, ':');
- i=0;
- do
- {
- ReadUntil (file, &buffer[i], 1, poll_timeout);
- i++;
- } while (buffer[i-1] != ';');
- buffer[i-1] = 0;
- cfg->max_angle = atol ((const char*)buffer);
- cfg->max_angle -= 384.0;
- cfg->max_angle *= cfg->resolution;
-
- ReadUntil_nthOccurence (file, 4, (char)0xa);
-
- if (verbose)
- {
- fprintf (stderr, "urg_nz: I: URG-04 specifications: [min_angle, max_angle, resolution, max_range] = [%f, %f, %f, %f]\n",
- cfg->min_angle, cfg->max_angle, cfg->resolution, cfg->max_range);
- }
- }
-}
-
-///////////////////////////////////////////////////////////////////////////////
-int urg_laser::GetSCIPVersion (void)
-{
- unsigned char buffer [18];
- memset (buffer, 0, 18);
- int file = fileno (laser_port);
- /////////////////
- // try SCIP1 first:
- /////////////////
- tcflush (fileno (laser_port), TCIFLUSH);
- fprintf (laser_port, "V\n");
-
- // check the returned command
- memset (buffer, 0, 18);
- ReadUntil (file, buffer, 4, poll_timeout);
-
- if (strncmp ((const char *) buffer, "V\n0\n", 4) != 0)
- {
- // SCIP1.0 failed, so we test it with SCIP2.0:
- tcflush (fileno (laser_port), TCIFLUSH);
- fprintf (laser_port, "VV\n");
-
- int file = fileno (laser_port);
-
- // check the returned command
- memset (buffer, 0, 18);
- ReadUntil (file, buffer, 7, poll_timeout);
- tcflush (fileno (laser_port), TCIFLUSH);
-
- if (strncmp ((const char *) buffer, "VV\n00P\n", 7) != 0)
- {
- stringstream error_desc;
- error_desc << "Error reading after VV command. Answer: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // Set SCIP version 2 and return
- SCIP_Version = 2;
- return SCIP_Version;
- }
-
- // we are currently in SCIP 1.0
- else
- {
- buffer[0] = 0;
- // Read the rest of the values, up till right before firmware version
- ReadUntil_nthOccurence (file, 2, (char)0xa);
- // Read "FIRM:"
- memset (buffer, 0, 18);
- ReadUntil (file, buffer, 5, poll_timeout);
-
- if (strncmp ((const char *) buffer, "FIRM:", 5) != 0)
- {
- stringstream error_desc;
- error_desc << "FIRM: is not where it is supposed to be: " << buffer;
- throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
- }
-
- // Read the firmware version major value
- ReadUntil (file, buffer, 5, poll_timeout);
- buffer[1] = 0;
- int firmware = atol ((const char*)buffer);
- if (verbose)
- fprintf (stderr, "urg_nz: I: Firmware major version is %d\n", firmware);
-
- ReadUntil_nthOccurence (file, 4, (char)0xa);
- if (firmware < 3)
- {
- // Set SCIP version 1 and return
- SCIP_Version = 1;
- return SCIP_Version;
- }
- else
- {
- // try to switch to SCIP2.0
- tcflush (fileno (laser_port), TCIFLUSH);
- fprintf (laser_port, "SCIP2.0\n");
-
- // check the returned command
- memset (buffer, 0, 18);
- ReadUntil (file, buffer, 2, poll_timeout);
- if (strncmp ((const char *) buffer, "SC", 2) != 0)
- {
- // Set SCIP version 1 and return
- SCIP_Version = 1;
- return SCIP_Version;
- }
- else
- {
- memset (&buffer[2], 0, 16);
- ReadUntil (file, &buffer[2], 8, poll_timeout);
- if (strncmp ((const char *) buffer, "SCIP2.0\n0\n", 11) != 0)
- {
- // Set SCIP version 1 and return
- SCIP_Version = 1;
- return SCIP_Version;
- }
- // Set SCIP version 2, turn laser on and return
- SCIP_Version = 2;
- fprintf (laser_port, "BM\n");
- ReadUntil_nthOccurence (file, 3, (char)0xa);
- tcflush (fileno (laser_port), TCIFLUSH);
- return SCIP_Version;
- }
- }
- }
-
- // Shouldn't get here
- stringstream error_desc;
- error_desc << "Unknown SCIP version";
- throw urg_nz_exception (URG_ERR_SCIPVERSION, error_desc.str ());
-}
Copied: gearbox/trunk/retired/urg_nz/urg_nz.cpp (from rev 271, gearbox/trunk/src/urg_nz/urg_nz.cpp)
===================================================================
--- gearbox/trunk/retired/urg_nz/urg_nz.cpp (rev 0)
+++ gearbox/trunk/retired/urg_nz/urg_nz.cpp 2008-07-10 00:15:51 UTC (rev 272)
@@ -0,0 +1,991 @@
+/*
+ * Hokuyo URG laser scanner driver class.
+ * Originally written by Toby Collett for the Player project.
+ */
+
+#include <assert.h>
+#include <stdio.h>
+#include <string.h>
+#include <unistd.h>
+#include <errno.h>
+#include <stdlib.h>
+#include <fcntl.h>
+#include <termios.h>
+#include <poll.h>
+#include <sstream>
+
+#include "urg_nz.h"
+
+using namespace urg_nz;
+using namespace std;
+
+#ifndef M_PI
+ #define M_PI 3.14159265358979323846
+#endif
+// Convert radians to degrees
+#ifndef RTOD
+ #define RTOD(r) ((r) * 180 / M_PI)
+#endif
+// Convert degrees to radians
+#ifndef DTOR
+ #define DTOR(d) ((d) * M_PI / 180)
+#endif
+
+///////////////////////////////////////////////////////////////////////////////
+// Constructor
+///////////////////////////////////////////////////////////////////////////////
+urg_laser::urg_laser (void)
+{
+ // Defaults to SCIP version 1
+ SCIP_Version = 1;
+ laser_port = NULL;
+ verbose = false;
+ poll_timeout = -1;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Destructor
+///////////////////////////////////////////////////////////////////////////////
+urg_laser::~urg_laser (void)
+{
+ if (PortOpen ())
+ Close ();
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Read functions
+///////////////////////////////////////////////////////////////////////////////
+int urg_laser::ReadUntil_nthOccurence (int file, int n, char c)
+{
+ int retval = 0, bytes_read = 0;
+ unsigned char buffer[2];
+ buffer[0] = 0;
+ buffer[1] = 0;
+ for (int i = 0; i < n; i++)
+ {
+ do
+ {
+ retval = ReadUntil (file, &buffer[0], 1, poll_timeout);
+ if (retval > 0)
+ bytes_read += retval;
+ } while (buffer[0] != c && retval > 0);
+ }
+ return bytes_read;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+int urg_laser::ReadUntil (int fd, unsigned char *buf, int len, int timeout)
+{
+ int ret;
+ int current=0;
+ struct pollfd ufd[1];
+ int retval;
+
+ ufd[0].fd = fd;
+ ufd[0].events = POLLIN;
+
+ do
+ {
+ if(timeout >= 0)
+ {
+ if ((retval = poll (ufd, 1, timeout)) < 0)
+ {
+ stringstream error_desc;
+ error_desc << "Error in ReadUntil at poll(): " << errno << ":" << strerror (errno);
+ throw urg_nz_exception (URG_ERR_READPOLL, error_desc.str ());
+ }
+ else if (retval == 0)
+ {
+ stringstream error_desc;
+ error_desc << "Timed out on poll in ReadUntil";
+ throw urg_nz_exception (URG_ERR_READTIMEOUT, error_desc.str ());
+ }
+ }
+
+ ret = read (fd, &buf[current], len-current);
+ if (ret < 0)
+ {
+ stringstream error_desc;
+ error_desc << "Error in ReadUntil at read(): " << errno << ":" << strerror (errno);
+ throw urg_nz_exception (URG_ERR_READ, error_desc.str ());
+ }
+
+ current += ret;
+ if (current > 2 && current < len && buf[current-2] == '\n' && buf[current-1] == '\n')
+ {
+ stringstream error_desc;
+ error_desc << "Got an end of command while waiting for more data.";
+ throw urg_nz_exception (URG_ERR_PROTOCOL, error_desc.str ());
+ }
+ } while (current < len);
+ return current;
+}
+
+///////////////////////////////////////////////////////////////////////////////
+// Port control functions
+///////////////////////////////////////////////////////////////////////////////
+int urg_laser::ChangeBaud (int curr_baud, int new_baud)
+{
+ struct termios newtio;
+ int fd;
+ fd = fileno (laser_port);
+ int new_baud_constant;
+
+ switch (curr_baud)
+ {
+ case 19200:
+ curr_baud = B19200;
+ break;
+ case 57600:
+ curr_baud = B57600;
+ break;
+ case 115200:
+ curr_baud = B115200;
+ break;
+ default:
+ stringstream error_desc;
+ error_desc << "Unknown current baud rate: " << curr_baud;
+ throw urg_nz_exception (URG_ERR_BADBAUDRATE, error_desc.str ());
+ }
+
+ switch (new_baud)
+ {
+ case 19200:
+ new_baud_constant = B19200;
+ break;
+ case 57600:
+ new_baud_constant = B57600;
+ break;
+ case 115200:
+ new_baud_constant = B115200;
+ break;
+ default:
+ stringstream error_desc;
+ error_desc << "Unknown new baud rate: " << new_baud;
+ throw urg_nz_exception (URG_ERR_BADBAUDRATE, error_desc.str ());
+ }
+
+ if (tcgetattr (fd, &newtio) < 0)
+ {
+ close (fd);
+ stringstream error_desc;
+ error_desc << "Error in ChangeBaud at tcgetattr: " << errno << ":" << strerror (errno);
+ throw urg_nz_exception (URG_ERR_CHANGEBAUD, error_desc.str ());
+ }
+
+ cfmakeraw (&newtio);
+ cfsetispeed (&newtio, curr_baud);
+ cfsetospeed (&newtio, curr_baud);
+
+ if (tcsetattr (fd, TCSAFLUSH, &newtio) < 0 )
+ {
+ close (fd);
+ stringstream error_desc;
+ error_desc << "Error in ChangeB...
[truncated message content] |