Menu
▾
▴
[Gearbox-commit] SF.net SVN: gearbox: [135] gearbox/trunk/src/gbxsickacfr
|
From: <bo...@us...> - 2008-05-04 07:38:48
|
Revision: 135
http://gearbox.svn.sourceforge.net/gearbox/?rev=135&view=rev
Author: borax00
Date: 2008-05-04 00:38:55 -0700 (Sun, 04 May 2008)
Log Message:
-----------
improved reliability.
Modified Paths:
--------------
gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
gearbox/trunk/src/gbxsickacfr/driver.cpp
gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
gearbox/trunk/src/gbxsickacfr/messages.cpp
gearbox/trunk/src/gbxsickacfr/serialhandler.h
gearbox/trunk/src/gbxsickacfr/test/test.cpp
Modified: gearbox/trunk/src/gbxsickacfr/CMakeLists.txt
===================================================================
--- gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/CMakeLists.txt 2008-05-04 07:38:55 UTC (rev 135)
@@ -33,8 +33,8 @@
GBX_ADD_HEADERS( gbxsickacfr ${hdrs} )
- IF( GBX_BUILD_TESTS )
+# IF( GBX_BUILD_TESTS )
ADD_SUBDIRECTORY( test )
- ENDIF( GBX_BUILD_TESTS )
+# ENDIF( GBX_BUILD_TESTS )
ENDIF( build )
Modified: gearbox/trunk/src/gbxsickacfr/driver.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/driver.cpp 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/driver.cpp 2008-05-04 07:38:55 UTC (rev 135)
@@ -129,27 +129,37 @@
ssDebug << "Connecting to laser on serial port " << config_.device;
tracer_.debug( ssDebug.str() );
- serialHandler_.reset( new SerialHandler( config_.device, tracer, status ) );
+ const int MAX_TRIES=3;
+ for ( int i=0; i < MAX_TRIES; i++ )
+ {
+ stringstream tryString;
+ tryString << "Connection attempt " << i+1 << " of " << MAX_TRIES << ": ";
+ try {
- try {
-
- initLaser();
-
+ serialHandler_.reset(0);
+ serialHandler_.reset( new SerialHandler( config_.device, tracer, status ) );
+ initLaser();
+ break;
+ }
+ catch ( const ResponseIsErrorException &e )
+ {
+ std::string errorLog = errorConditions();
+ stringstream ss;
+ ss << e.what() << endl << "Laser error log: " << errorLog;
+ if ( i == MAX_TRIES-1 )
+ throw ResponseIsErrorException( ss.str() );
+ else
+ tracer_.warning( tryString.str() + ss.str() );
+ }
+ catch ( const std::exception &e )
+ {
+ stringstream ss;
+ ss << "during initLaser(): " << e.what();
+ tracer_.warning( tryString.str() + ss.str() );
+ if ( i == MAX_TRIES-1 )
+ throw;
+ }
}
- catch ( const ResponseIsErrorException &e )
- {
- std::string errorLog = errorConditions();
- stringstream ss;
- ss << e.what() << endl << "Laser error log: " << errorLog;
- throw ResponseIsErrorException( ss.str() );
- }
- catch ( const std::exception &e )
- {
- stringstream ss;
- ss << "during initLaser(): " << e.what();
- tracer_.warning( ss.str() );
- throw;
- }
}
bool
@@ -333,6 +343,7 @@
Driver::errorConditions()
{
try {
+ tracer_.debug( "Driver: Checking error conditions." );
constructRequestErrorMessage( commandAndData_ );
const bool ignoreErrorConditions = true;
TimedLmsResponse errorResponse = sendAndExpectResponse( commandAndData_, ignoreErrorConditions );
@@ -350,8 +361,10 @@
void
Driver::setBaudRate( int baudRate )
{
+ // Tell the laser to switch
constructRequestBaudRate( commandAndData_, baudRate );
sendAndExpectResponse( commandAndData_ );
+ // And switch myself
serialHandler_->setBaudRate( config_.baudRate );
}
@@ -372,26 +385,21 @@
ss << "Driver: Trying to connect at " << baudRates[baudRateI] << " baud.";
tracer_.info( ss.str() );
+ // Switch my local serial port
serialHandler_->setBaudRate( baudRates[baudRateI] );
- const uint MAX_TRIES = 2;
- for ( uint tryNum=0; tryNum < MAX_TRIES; tryNum++ )
+ try {
+ stringstream ss;
+ ss <<"Driver: Trying to get laser status with baudrate " << baudRates[baudRateI];
+ tracer_.debug( ss.str() );
+ askLaserForStatusData();
+ return baudRates[baudRateI];
+ }
+ catch ( const NoResponseException &e )
{
- try {
- stringstream ss;
- ss <<"Driver: Trying to get laser status with baudrate " << baudRates[baudRateI] << " (try number "<<tryNum<<" of " << MAX_TRIES << ")" << endl;
- tracer_.debug( ss.str() );
- askLaserForStatusData();
- return baudRates[baudRateI];
- }
- catch ( const NoResponseException &e )
- {
- stringstream ss;
- ss << "Driver::guessLaserBaudRate(): try " << tryNum << " of " << MAX_TRIES << " at baudRate " << baudRates[baudRateI] << " failed: " << e.what();
- tracer_.debug( ss.str() );
- if ( tryNum == MAX_TRIES-1 )
- break;
- }
+ stringstream ss;
+ ss << "Driver::guessLaserBaudRate(): failed: " << e.what();
+ tracer_.debug( ss.str() );
}
} // end loop over baud rates
@@ -408,30 +416,13 @@
// Turn continuous mode off
//
tracer_.debug("Driver: Turning continuous mode off");
- // For some reason this isn't always reliable, not too sure why.
- // Perhaps there's some crap left in the buffer after the thing
- // was previously in continuous mode?
- const int MAX_TRIES=3;
- for ( int i=0; i < MAX_TRIES; i++ )
- {
- try {
- constructRequestMeasuredOnRequestMode( commandAndData_ );
- sendAndExpectResponse( commandAndData_ );
- break;
- }
- catch ( NoResponseException &e )
- {
- if ( i == MAX_TRIES-1 )
- {
- // Give up
- throw;
- }
- }
- }
+ constructRequestMeasuredOnRequestMode( commandAndData_ );
+ sendAndExpectResponse( commandAndData_ );
//
// Set Desired BaudRate
//
+ tracer_.debug("Driver: Telling the laser to switch to the desired baud rate");
if ( currentBaudRate != config_.baudRate )
{
setBaudRate( config_.baudRate );
@@ -449,6 +440,7 @@
//
// Check operating data counters
//
+ tracer_.debug("Driver: Checking operating data counters");
constructRequestOperatingDataCounter( commandAndData_ );
TimedLmsResponse counterResponse = sendAndExpectResponse( commandAndData_ );
ssInfo << "OperatingDataCounter: " << toString(counterResponse.response) << endl;
@@ -456,6 +448,7 @@
//
// Get status
//
+ tracer_.debug("Driver: Checking status");
LmsResponse statusResponse = askLaserForStatusData();
LmsStatusResponseData *statusResponseData =
dynamic_cast<LmsStatusResponseData*>(statusResponse.data);
Modified: gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/gbxserialdeviceacfr/serialdevicehandler.cpp 2008-05-04 07:38:55 UTC (rev 135)
@@ -71,7 +71,7 @@
void
SerialDeviceHandler::setBaudRate( int baudRate )
{
- tracer_.debug( "SerialDeviceHandler: Changing baud rate and flushing." );
+ tracer_.debug( "SerialDeviceHandler: Changing baud rate of serial port." );
serial_.setBaudRate( baudRate );
// TODO: AlexB: not entirely sure if these are
// necessary, they should either be removed or
@@ -161,7 +161,7 @@
if ( SUPER_DEBUG )
{
stringstream ss;
- ss << "SerialDeviceHandler::getDataFromSerial(): nBytes: " << nBytes;
+ ss << "SerialDeviceHandler::getDataFromSerial(): nBytes available: " << nBytes;
tracer_.debug( ss.str(), 9 );
}
Modified: gearbox/trunk/src/gbxsickacfr/messages.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/messages.cpp 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/messages.cpp 2008-05-04 07:38:55 UTC (rev 135)
@@ -893,10 +893,10 @@
if ( checksumFailed )
{
IceUtil::Time t = IceUtil::Time::now();
- cout << "WARN(messages.cpp): " << t.toDateTime() << ": Checksum failed at buf pos " <<bytesParsed << endl;
- cout<<"TRACE(messages.cpp): checksum was over: " << toHexString( &(buffer[bytesParsed]), telegramLength ) << endl;
+ // cout << "WARN(messages.cpp): " << t.toDateTime() << ": Checksum failed at buf pos " <<bytesParsed << endl;
+ // cout<<"TRACE(messages.cpp): checksum was over: " << toHexString( &(buffer[bytesParsed]), telegramLength ) << endl;
bytesParsed++;
- cout<<"TRACE(messages.cpp): returning after failed checksum, with bytesParsed = " << bytesParsed << endl;
+ // cout<<"TRACE(messages.cpp): returning after failed checksum, with bytesParsed = " << bytesParsed << endl;
continue;
}
else
Modified: gearbox/trunk/src/gbxsickacfr/serialhandler.h
===================================================================
--- gearbox/trunk/src/gbxsickacfr/serialhandler.h 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/serialhandler.h 2008-05-04 07:38:55 UTC (rev 135)
@@ -68,8 +68,8 @@
public:
SerialHandler( const std::string &dev,
- gbxutilacfr::Tracer &tracer,
- gbxutilacfr::Status &status );
+ gbxutilacfr::Tracer &tracer,
+ gbxutilacfr::Status &status );
~SerialHandler();
void send( const std::vector<uChar> &telegram )
@@ -99,7 +99,7 @@
private:
- ResponseParser responseParser_;
+ ResponseParser responseParser_;
gbxserialacfr::Serial serialPort_;
gbxserialdeviceacfr::SerialDeviceHandler *serialDeviceHandler_;
// Keep a smart pointer to the SerialDeviceHandler as a thread, for stop/start purposes
Modified: gearbox/trunk/src/gbxsickacfr/test/test.cpp
===================================================================
--- gearbox/trunk/src/gbxsickacfr/test/test.cpp 2008-05-01 00:05:33 UTC (rev 134)
+++ gearbox/trunk/src/gbxsickacfr/test/test.cpp 2008-05-04 07:38:55 UTC (rev 135)
@@ -31,9 +31,11 @@
int opt;
int baud = 38400;
string port = "/dev/ttyS0";
+ bool debug = false;
+ bool showScan = false;
// Get some options from the command line
- while ((opt = getopt(argc, argv, "p:b:")) != -1)
+ while ((opt = getopt(argc, argv, "p:b:vs")) != -1)
{
switch ( opt )
{
@@ -43,10 +45,16 @@
case 'b':
baud = atoi( optarg );
break;
+ case 'v':
+ debug = true;
+ break;
+ case 's':
+ showScan = true;
+ break;
default:
- cout << "Usage: " << argv[0] << " [-p port] [-b baud]" << endl << endl
+ cout << "Usage: " << argv[0] << " [-p port] [-b baud] [-v(erbose)] [-s(how scan)]" << endl << endl
<< "-p port\tPort the laser scanner is connected to. E.g. /dev/ttyS0" << endl
- << "-b baud\tBaud rate to connect at (9600, 19200, 38400, oro 500000)." << endl;
+ << "-b baud\tBaud rate to connect at (9600, 19200, 38400, or 500000)." << endl;
return 1;
}
}
@@ -67,7 +75,6 @@
cout << "Using configuration: " << config.toString() << endl;
// Instantiate objects to handle messages from the driver
- const bool debug=false;
gbxsickacfr::gbxutilacfr::TrivialTracer tracer( debug );
gbxsickacfr::gbxutilacfr::TrivialStatus status( tracer );
@@ -99,14 +106,16 @@
device->read( data );
cout<<"Test: Got scan "<<i+1<<" of "<<numReads<<endl;
- for ( int i=0; i < config.numberOfSamples; i++ )
+ if ( showScan )
{
- const double angle = config.startAngle + i*config.fieldOfView/(double)(config.numberOfSamples-1);
- cout << " " << i << ": angle=" << angle*180.0/M_PI
- << "deg, range=" << data.ranges[i]
- << ", intensity=" << data.intensities[i] << endl;
+ for ( int i=0; i < config.numberOfSamples; i++ )
+ {
+ const double angle = config.startAngle + i*config.fieldOfView/(double)(config.numberOfSamples-1);
+ cout << " " << i << ": angle=" << angle*180.0/M_PI
+ << "deg, range=" << data.ranges[i]
+ << ", intensity=" << data.intensities[i] << endl;
+ }
}
-
if ( data.haveWarnings )
cout << "got warnings: " << data.warnings << endl;
}
This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.
|