[Gearbox-commit] SF.net SVN: gearbox:[367] gearbox/trunk/src/hokuyo_aist

[<gb...@us...>]

Revision: 367
          http://gearbox.svn.sourceforge.net/gearbox/?rev=367&view=rev
Author:   gbiggs
Date:     2008-12-19 08:11:45 +0000 (Fri, 19 Dec 2008)

Log Message:
-----------
Added baud rate probing for serial connections to hokuyo_aist, and allowed baud rate 38400

Modified Paths:
--------------
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
    gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2008-11-29 12:16:54 UTC (rev 366)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.cpp	2008-12-19 08:11:45 UTC (rev 367)
@@ -930,6 +930,91 @@
 	GetDefaults ();
 }
 
+unsigned int HokuyoLaser::OpenWithProbing (string portOptions)
+{
+	if (_verbose)
+	{
+		cerr << "HokuyoLaser::" << __func__ << "() Creating and opening port using options: " <<
+			portOptions << endl;
+	}
+	_port = flexiport::CreatePort (portOptions);
+	_port->Open ();
+
+	if (_verbose)
+	{
+		cerr << "HokuyoLaser::" << __func__ << "() Connected using " << _port->GetPortType () <<
+			" connection." << endl;
+		cerr << _port->GetStatus ();
+	}
+	_port->Flush ();
+
+	try
+	{
+		// Figure out the SCIP version currently in use and switch to a higher one if possible
+		GetAndSetSCIPVersion ();
+		// Get some values we need for providing default ranges
+		GetDefaults ();
+	}
+	catch (HokuyoError)
+	{
+		if (_verbose)
+		{
+			cerr << "HokuyoLaser::" << __func__ <<
+				"() Failed to connect at the default baud rate." << endl;
+		}
+		if (_port->GetPortType () == "serial")
+		{
+			// Failed at the default baud rate, so try again at the other rates
+			const unsigned int bauds[] = {750000, 500000, 250000, 115200, 57600, 38400, 19200};
+			const unsigned int numBauds = 7;
+			for (unsigned int ii = 0; ii < numBauds; ii++)
+			{
+				reinterpret_cast<SerialPort*> (_port)->SetBaudRate (bauds[ii]);
+				try
+				{
+					GetAndSetSCIPVersion ();
+					GetDefaults ();
+					// If the above two functions succeed, break out of the loop and be happy
+					if (_verbose)
+					{
+						cerr << "HokuyoLaser::" << __func__ << "() Connected at " <<
+							bauds[ii] << endl;
+					}
+					return bauds[ii];
+				}
+				catch (HokuyoError)
+				{
+					if (ii == numBauds - 1)
+					{
+						// Last baud rate, give up and rethrow
+						if (_verbose)
+						{
+							cerr << "HokuyoLaser::" << __func__ <<
+								"() Failed to connect at any baud rate." << endl;
+						}
+						throw;
+					}
+					// Otherwise go around again
+				}
+			}
+		}
+		else
+		{
+			if (_verbose)
+			{
+				cerr << "HokuyoLaser::" << __func__ << "() Port is not serial, cannot probe." <<
+					endl;
+			}
+			throw;
+		}
+	}
+
+	if (_port->GetPortType () == "serial")
+		return reinterpret_cast<SerialPort*> (_port)->GetBaudRate ();
+	else
+		return 0;
+}
+
 void HokuyoLaser::Close (void)
 {
 	if (!_port)
@@ -997,8 +1082,8 @@
 	char newBaud[13];
 	memset (newBaud, 0, sizeof (char) * 13);
 
-	if (baud != 19200 && baud != 57600 && baud != 115200 &&
-			baud != 250000 && baud != 500000 && baud != 750000)
+	if (baud != 19200 && baud != 38400 && baud != 57600 && baud != 115200 &&
+		baud != 250000 && baud != 500000 && baud != 750000)
 	{
 		stringstream ss;
 		ss << "Bad baud rate: " << baud << endl;

Modified: gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h
===================================================================
--- gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2008-11-29 12:16:54 UTC (rev 366)
+++ gearbox/trunk/src/hokuyo_aist/hokuyo_aist.h	2008-12-19 08:11:45 UTC (rev 367)
@@ -306,6 +306,16 @@
 		/// @brief Open the laser scanner and begin scanning.
 		void Open (std::string portOptions);
 
+		/** @brief Open the laser scanner and begin scanning, probing the baud rate as necessary.
+
+		If the port is a serial connection and communication with the laser fails at the given
+		baud rate, the alternative baud rates supported by the device are tried (see @ref SetBaud
+		for these) in order from fastest to slowest.
+
+		@return The baud rate at which connection with the laser succeeded, or 0 for non-serial
+		connections. */
+		unsigned int OpenWithProbing (std::string portOptions);
+
 		/// @brief Close the connection to the laser scanner.
 		void Close (void);
 
@@ -317,7 +327,7 @@
 
 		/** @brief Change the baud rate when using a serial connection.
 
-		Valid baud rates are: 19.2Kbps, 57.6Kbps, 115.2Kbps, 250.0Kbps, 500.0Kbps, 750.0Kbps
+		Valid rates are 19.2Kbps, 38.4Kbps, 57.6Kbps, 115.2Kbps, 250.0Kbps, 500.0Kbps, 750.0Kbps
 		(dependent on those available in FlexiPort). */
 		void SetBaud (unsigned int baud);
 


This was sent by the SourceForge.net collaborative development platform, the world's largest Open Source development site.