Detailed Description

This software driver implements the protocol SCIP-2.0 for interfacing HOKUYO URG, UTM and UXM laser scanners.

Refer to the wiki page for more details: http://www.mrpt.org/Example:HOKUYO_URG/UTM_Laser_Scanner

See also the application "rawlog-grabber" for a ready-to-use application to gather data from the scanner.

  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
 -------------------------------------------------------
   [supplied_section_name]
    HOKUYO_motorSpeed_rpm=600
    //HOKUYO_HS_mode   = false    // Optional (un-comment line if used): Set/unset the High-sensitivity mode (not on all models/firmwares!)
    COM_port_WIN = COM3
    COM_port_LIN = ttyS0
    pose_x=0.21 // Laser range scaner 3D position in the robot (meters)
    pose_y=0
    pose_z=0.34
    pose_yaw=0  // Angles in degrees
    pose_pitch=0
    pose_roll=0
    //IP_DIR    =       192.168.0.10 // Uncommented this and "PORT_DIR" if the used HOKUYO is connected by Ethernet instead of USB
    //PORT_DIR = 10940

    // Optional: reduced FOV:
    // reduced_fov  = 25 // Deg

    // Optional: Exclusion zones to avoid the robot seeing itself:
    //exclusionZone1_x = 0.20 0.30 0.30 0.20
    //exclusionZone1_y = 0.20 0.30 0.30 0.20

    // Optional: Exclusion zones to avoid the robot seeing itself:
    //exclusionAngles1_ini = 20  // Deg
    //exclusionAngles1_end = 25  // Deg

#include <mrpt/hwdrivers/CHokuyoURG.h>

Inheritance diagram for mrpt::hwdrivers::CHokuyoURG:

[legend]

List of all members.

Classes

struct TSensorInfo

Used in CHokuyoURG::displayVersionInfo. More...

Public Types

enum TSensorState { ssInitializing = 0, ssWorking, ssError }

The current state of the sensor. More...

typedef std::multimap
< mrpt::system::TTimeStamp,
mrpt::utils::CSerializablePtr > TListObservations

typedef std::pair
< mrpt::system::TTimeStamp,
mrpt::utils::CSerializablePtr > TListObsPair

Public Member Functions

CHokuyoURG ()

Constructor.

virtual ~CHokuyoURG ()

Destructor: turns the laser off.

void doProcessSimple (bool &outThereIsObservation, mrpt::slam::CObservation2DRangeScan &outObservation, bool &hardwareError)

Specific laser scanner "software drivers" must process here new data from the I/O stream, and, if a whole scan has arrived, return it.

bool turnOn ()

Enables the scanning mode (which may depend on the specific laser device); this must be called before asking for observations to assure that the protocol has been initializated.

bool turnOff ()

Disables the scanning mode (this can be used to turn the device in low energy mode, if available)

void purgeBuffers ()

Empties the RX buffers of the serial port.

void setSerialPort (const std::string &port_name)

If set to non-empty, the serial port will be attempted to be opened automatically when this class is first used to request data from the laser.

void setIPandPort (const std::string &ip, const unsigned int &port)

Set the ip direction and port to connect using Ethernet communication.

const std::string getSerialPort ()

Returns the currently set serial port.

void setReducedFOV (const double fov)

If called (before calling "turnOn"), the field of view of the laser is reduced to the given range (in radians), discarding the rest of measures.

bool setHighSensitivityMode (bool enabled)

Changes the high sensitivity mode (HS) (default: false)

void setVerbose (bool enable=true)

void bindIO (CStream *streamIO)

Binds the object to a given I/O channel.

void getObservation (bool &outThereIsObservation, CObservation2DRangeScan &outObservation, bool &hardwareError)

Get the last observation from the sensor, if available, and unmarks it as being "the last one" (thus a new scan must arrive or subsequent calls will find no new observations).

void doProcess ()

Main method for a CGenericSensor.

virtual const
mrpt::hwdrivers::TSensorClassId * GetRuntimeClass () const =0

TSensorState getState () const

The current state of the sensor.

double getProcessRate () const

std::string getSensorLabel () const

void setSensorLabel (const std::string &sensorLabel)

void loadConfig (const mrpt::utils::CConfigFileBase &configSource, const std::string &section)

Loads the generic settings common to any sensor (See CGenericSensor), then call to "loadConfig_sensorSpecific"

Exceptions:

This	method throws an exception with a descriptive message if some critical parameter is missing or has an invalid value.

void getObservations (TListObservations &lstObjects)

Returns a list of enqueued objects, emptying it (thread-safe).

virtual void setPathForExternalImages (const std::string &directory)

Set the path where to save off-rawlog image files (will be ignored in those sensors where this is not applicable).

void setExternalImageFormat (const std::string &ext)

Set the extension ("jpg","gif","png",...) that determines the format of images saved externally The default is "jpg".

void setExternalImageJPEGQuality (const unsigned int quality)

The quality of JPEG compression, when external images is enabled and the format is "jpg".

unsigned int getExternalImageJPEGQuality () const

Static Public Member Functions

static void printf_debug (const char *frmt,...)

Sends a formated text to "debugOut" if not NULL, or to cout otherwise.

static void registerClass (const TSensorClassId *pNewClass)

Register a class into the internal list of "CGenericSensor" descendents.

static CGenericSensor * createSensor (const std::string &className)

Creates a sensor by a name of the class.

static CGenericSensorPtr createSensorPtr (const std::string &className)

Just like createSensor, but returning a smart pointer to the newly created sensor object.

Protected Member Functions

bool checkCOMisOpen ()

Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open the serial port "m_com_port".

void loadConfig_sensorSpecific (const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)

Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes) See hwdrivers::CHokuyoURG for the possible parameters.

void loadExclusionAreas (const mrpt::utils::CConfigFileBase &configSource, const std::string &iniSection)

Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles).

void filterByExclusionAreas (CObservation2DRangeScan &obs) const

Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.

void filterByExclusionAngles (CObservation2DRangeScan &obs) const

Mark as invalid those ranges in a set of forbiden angle ranges.

void appendObservations (const std::vector< mrpt::utils::CSerializablePtr > &obj)

This method must be called by derived classes to enqueue a new observation in the list to be returned by getObservations.

void appendObservation (const mrpt::utils::CSerializablePtr &obj)

Like appendObservations() but for just one observation.

Protected Attributes

double m_reduced_fov

Used to reduce artificially the interval of scan ranges.

std::string

m_com_port

If set to non-empty, the serial port will be attempted to be opened automatically when this class is first used to request data from the laser.

std::string

m_ip_dir

If set to non-empty and m_port_dir too, the program will try to connect to a Hokuyo using Ethernet communication.

unsigned int m_port_dir

If set to non-empty and m_ip_dir too, the program will try to connect to a Hokuyo using Ethernet communication.

TSensorInfo

m_sensor_info

The information gathered when the laser is first open.

bool m_I_am_owner_serial_port

uint32_t m_timeStartUI

Time of the first data packet, for synchronization purposes.

mrpt::system::TTimeStamp

m_timeStartTT

utils::CStream * m_stream

The I/O channel (will be NULL if not bound).

size_t m_grab_decimation_counter

Used when "m_grab_decimation" is enabled.

TSensorState

m_state

std::string

m_path_for_external_images

The path where to save off-rawlog images: empty means save images embedded in the rawlog.

std::string

m_external_images_format

The extension ("jpg","gif","png",...) that determines the format of images saved externally.

unsigned int m_external_images_jpeg_quality

For JPEG images, the quality (default=95%).

Common settings to any sensor, loaded in "loadConfig"

double m_process_rate

See CGenericSensor.

size_t m_max_queue_len

See CGenericSensor.

size_t m_grab_decimation

If set to N>=2, only 1 out of N observations will be saved to m_objList.

std::string

m_sensorLabel

See CGenericSensor.

Private Member Functions

bool enableSCIP20 ()

Enables the SCIP2.0 protocol (this must be called at the very begining!).

bool setHighBaudrate ()

Passes to 115200bps bitrate.

bool switchLaserOn ()

Switchs the laser on.

bool switchLaserOff ()

Switchs the laser off.

bool setMotorSpeed (int motoSpeed_rpm)

Changes the motor speed in rpm's (default 600rpm)

bool displayVersionInfo ()

Ask to the device, and print to the debug stream, details about the firmware version,serial number,...

bool displaySensorInfo (CHokuyoURG::TSensorInfo *out_data=NULL)

Ask to the device, and print to the debug stream, details about the sensor model.

bool startScanningMode ()

Start the scanning mode, using parameters stored in the object (loaded from the .ini file) After this command the device will start to send scans until "switchLaserOff" is called.

void initialize ()

Turns the laser on.

bool receiveResponse (const char *sentCmd_forEchoVerification, char &rcv_status0, char &rcv_status1, char *rcv_data, int &rcv_dataLength)

Waits for a response from the device.

bool assureBufferHasBytes (const size_t nDesiredBytes)

Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.

Private Attributes

int m_firstRange

int m_lastRange

The first and last ranges to consider from the scan.

int m_motorSpeed_rpm

The motor speed (default=600rpm)

poses::CPose3D

m_sensorPose

The sensor 6D pose:

mrpt::utils::circular_buffer
< uint8_t > m_rx_buffer

Auxiliary buffer for readings.

std::string

m_lastSentMeasCmd

The last sent measurement command (MDXXX), including the last 0x0A.

bool m_verbose

bool m_highSensMode

High sensitivity [HS] mode (default: false)

Member Typedef Documentation

typedef std::multimap< mrpt::system::TTimeStamp, mrpt::utils::CSerializablePtr > mrpt::hwdrivers::CGenericSensor::TListObservations [inherited]

Definition at line 84 of file CGenericSensor.h.

typedef std::pair< mrpt::system::TTimeStamp, mrpt::utils::CSerializablePtr > mrpt::hwdrivers::CGenericSensor::TListObsPair [inherited]

Definition at line 85 of file CGenericSensor.h.

Member Enumeration Documentation

enum mrpt::hwdrivers::CGenericSensor::TSensorState [inherited]

The current state of the sensor.

See also:: CGenericSensor::getState

Enumerator:

ssInitializing
ssWorking
ssError

Definition at line 90 of file CGenericSensor.h.

Constructor & Destructor Documentation

mrpt::hwdrivers::CHokuyoURG::CHokuyoURG ( )

Constructor.

virtual mrpt::hwdrivers::CHokuyoURG::~CHokuyoURG ( ) [virtual]

Destructor: turns the laser off.

Member Function Documentation

void mrpt::hwdrivers::CGenericSensor::appendObservation ( const mrpt::utils::CSerializablePtr & obj ) [inline, protected, inherited]

Like appendObservations() but for just one observation.

Definition at line 155 of file CGenericSensor.h.

void mrpt::hwdrivers::CGenericSensor::appendObservations ( const std::vector< mrpt::utils::CSerializablePtr > & obj ) [protected, inherited]

This method must be called by derived classes to enqueue a new observation in the list to be returned by getObservations.

Passed objects must be created in dynamic memory and a smart pointer passed. Example of creation:

                                CObservationGPSPtr  o = CObservationGPSPtr( new CObservationGPS() );
                                o-> .... // Set data
                                appendObservation(o);

If several observations are passed at once in the vector, they'll be considered as a block regarding the grabbing decimation factor.

bool mrpt::hwdrivers::CHokuyoURG::assureBufferHasBytes ( const size_t nDesiredBytes ) [private]

Assures a minimum number of bytes in the input buffer, reading from the serial port only if required.

Returns:: false if the number of bytes are not available, even after trying to fetch more data from the serial port.

void mrpt::hwdrivers::C2DRangeFinderAbstract::bindIO ( CStream * streamIO ) [inherited]

Binds the object to a given I/O channel.

The stream object must not be deleted before the destruction of this class.

See also:: hwdrivers::CSerialPort

bool mrpt::hwdrivers::CHokuyoURG::checkCOMisOpen ( ) [protected]

Returns true if there is a valid stream bound to the laser scanner, otherwise it first try to open the serial port "m_com_port".

static CGenericSensor* mrpt::hwdrivers::CGenericSensor::createSensor ( const std::string & className ) [static, inherited]

Creates a sensor by a name of the class.

Typically the user may want to create a smart pointer around the returned pointer, whis is made with:

   CGenericSensorPtr sensor = CGenericSensorPtr( CGenericSensor::createSensor("XXX") );

Returns:: A pointer to a new class, or NULL if class name is unknown.

static CGenericSensorPtr mrpt::hwdrivers::CGenericSensor::createSensorPtr ( const std::string & className ) [inline, static, inherited]

Just like createSensor, but returning a smart pointer to the newly created sensor object.

Definition at line 188 of file CGenericSensor.h.

bool mrpt::hwdrivers::CHokuyoURG::displaySensorInfo ( CHokuyoURG::TSensorInfo * out_data = NULL ) [private]

Ask to the device, and print to the debug stream, details about the sensor model.

It also optionally saves all the information in an user supplied data structure "out_data".

Returns:: false on any error

bool mrpt::hwdrivers::CHokuyoURG::displayVersionInfo ( ) [private]

Ask to the device, and print to the debug stream, details about the firmware version,serial number,...

Returns:: false on any error

void mrpt::hwdrivers::C2DRangeFinderAbstract::doProcess ( ) [virtual, inherited]

Main method for a CGenericSensor.

Implements mrpt::hwdrivers::CGenericSensor.

Reimplemented in mrpt::hwdrivers::CLMS100Eth.

void mrpt::hwdrivers::CHokuyoURG::doProcessSimple	(	bool &	outThereIsObservation,
		mrpt::slam::CObservation2DRangeScan &	outObservation,
		bool &	hardwareError
	)		`[virtual]`

Specific laser scanner "software drivers" must process here new data from the I/O stream, and, if a whole scan has arrived, return it.

This method will be typically called in a different thread than other methods, and will be called in a timely fashion.

Implements mrpt::hwdrivers::C2DRangeFinderAbstract.

bool mrpt::hwdrivers::CHokuyoURG::enableSCIP20 ( ) [private]

Enables the SCIP2.0 protocol (this must be called at the very begining!).

Returns:: false on any error

void mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAngles ( CObservation2DRangeScan & obs ) const [protected, inherited]

Mark as invalid those ranges in a set of forbiden angle ranges.

See also:: loadExclusionAreas

void mrpt::hwdrivers::C2DRangeFinderAbstract::filterByExclusionAreas ( CObservation2DRangeScan & obs ) const [protected, inherited]

Mark as invalid those points which (x,y) coordinates fall within the exclusion polygons.

See also:: loadExclusionAreas

unsigned int mrpt::hwdrivers::CGenericSensor::getExternalImageJPEGQuality ( ) const [inline, inherited]

Definition at line 241 of file CGenericSensor.h.

void mrpt::hwdrivers::C2DRangeFinderAbstract::getObservation	(	bool &	outThereIsObservation,
		CObservation2DRangeScan &	outObservation,
		bool &	hardwareError
	)		`[inherited]`

Get the last observation from the sensor, if available, and unmarks it as being "the last one" (thus a new scan must arrive or subsequent calls will find no new observations).

void mrpt::hwdrivers::CGenericSensor::getObservations ( TListObservations & lstObjects ) [inherited]

Returns a list of enqueued objects, emptying it (thread-safe).

The objects must be freed by the invoker.

double mrpt::hwdrivers::CGenericSensor::getProcessRate ( ) const [inline, inherited]

Definition at line 100 of file CGenericSensor.h.

virtual const mrpt::hwdrivers::TSensorClassId* mrpt::hwdrivers::CGenericSensor::GetRuntimeClass ( ) const [pure virtual, inherited]

std::string mrpt::hwdrivers::CGenericSensor::getSensorLabel ( ) const [inline, inherited]

Definition at line 102 of file CGenericSensor.h.

const std::string mrpt::hwdrivers::CHokuyoURG::getSerialPort ( ) [inline]

Returns the currently set serial port.

See also:: setSerialPort

Definition at line 199 of file CHokuyoURG.h.

TSensorState mrpt::hwdrivers::CGenericSensor::getState ( ) const [inline, inherited]

The current state of the sensor.

Definition at line 98 of file CGenericSensor.h.

void mrpt::hwdrivers::CHokuyoURG::initialize ( ) [private, virtual]

Turns the laser on.

Reimplemented from mrpt::hwdrivers::CGenericSensor.

void mrpt::hwdrivers::CGenericSensor::loadConfig	(	const mrpt::utils::CConfigFileBase &	configSource,
		const std::string &	section
	)		`[inherited]`

Loads the generic settings common to any sensor (See CGenericSensor), then call to "loadConfig_sensorSpecific"

Exceptions:

This	method throws an exception with a descriptive message if some critical parameter is missing or has an invalid value.

void mrpt::hwdrivers::CHokuyoURG::loadConfig_sensorSpecific	(	const mrpt::utils::CConfigFileBase &	configSource,
		const std::string &	iniSection
	)		`[protected, virtual]`

Loads specific configuration for the device from a given source of configuration parameters, for example, an ".ini" file, loading from the section "[iniSection]" (see utils::CConfigFileBase and derived classes) See hwdrivers::CHokuyoURG for the possible parameters.

Implements mrpt::hwdrivers::CGenericSensor.

void mrpt::hwdrivers::C2DRangeFinderAbstract::loadExclusionAreas	(	const mrpt::utils::CConfigFileBase &	configSource,
		const std::string &	iniSection
	)		`[protected, inherited]`

Should be call by derived classes at "loadConfig" (loads exclusion areas AND exclusion angles).

This loads a sequence of vertices of a polygon given by its (x,y) coordinates relative to the vehicle, that is, taking into account the "sensorPose".

exclusionZoneu_x
exclusionZoneu_y for u=1,2,3,... All points within the 2D polygon will be ignored, for any Z, unless an optional entry is found:
exclusionZoneu_z=[z_min z_max] In that case, only the points within the 2D polygon AND the given range in Z will be ignored.

The number of zones is variable, but they must start at 1 and be consecutive.

See also:: filterByExclusionAreas

static void mrpt::utils::CDebugOutputCapable::printf_debug	(	const char *	frmt,
			...
	)		`[static, inherited]`

Sends a formated text to "debugOut" if not NULL, or to cout otherwise.

Referenced by mrpt::math::CLevenbergMarquardtTempl::execute().

void mrpt::hwdrivers::CHokuyoURG::purgeBuffers ( )

Empties the RX buffers of the serial port.

bool mrpt::hwdrivers::CHokuyoURG::receiveResponse	(	const char *	sentCmd_forEchoVerification,
		char &	rcv_status0,
		char &	rcv_status1,
		char *	rcv_data,
		int &	rcv_dataLength
	)		`[private]`

Waits for a response from the device.

Returns:: false on any error

static void mrpt::hwdrivers::CGenericSensor::registerClass ( const TSensorClassId * pNewClass ) [static, inherited]

Register a class into the internal list of "CGenericSensor" descendents.

Used internally in the macros DEFINE_GENERIC_SENSOR, etc...

Can be used as "CGenericSensor::registerClass( SENSOR_CLASS_ID(CMySensor) );" if building custom sensors outside mrpt libraries in user code.

void mrpt::hwdrivers::CGenericSensor::setExternalImageFormat ( const std::string & ext ) [inline, inherited]

Set the extension ("jpg","gif","png",...) that determines the format of images saved externally The default is "jpg".

See also:: setPathForExternalImages, setExternalImageJPEGQuality

Definition at line 233 of file CGenericSensor.h.

void mrpt::hwdrivers::CGenericSensor::setExternalImageJPEGQuality ( const unsigned int quality ) [inline, inherited]

The quality of JPEG compression, when external images is enabled and the format is "jpg".

See also:: setExternalImageFormat

Definition at line 238 of file CGenericSensor.h.

bool mrpt::hwdrivers::CHokuyoURG::setHighBaudrate ( ) [private]

Passes to 115200bps bitrate.

Returns:: false on any error

bool mrpt::hwdrivers::CHokuyoURG::setHighSensitivityMode ( bool enabled )

Changes the high sensitivity mode (HS) (default: false)

Returns:: false on any error

void mrpt::hwdrivers::CHokuyoURG::setIPandPort	(	const std::string &	ip,
		const unsigned int &	port
	)		`[inline]`

Set the ip direction and port to connect using Ethernet communication.

Definition at line 196 of file CHokuyoURG.h.

bool mrpt::hwdrivers::CHokuyoURG::setMotorSpeed ( int motoSpeed_rpm ) [private]

Changes the motor speed in rpm's (default 600rpm)

Returns:: false on any error

virtual void mrpt::hwdrivers::CGenericSensor::setPathForExternalImages ( const std::string & directory ) [inline, virtual, inherited]

Set the path where to save off-rawlog image files (will be ignored in those sensors where this is not applicable).

An empty string (the default value at construction) means to save images embedded in the rawlog, instead of on separate files.

Exceptions:

std::exception If the directory doesn't exists and cannot be created.

Reimplemented in mrpt::hwdrivers::CKinect, mrpt::hwdrivers::CCameraSensor, and mrpt::hwdrivers::CSwissRanger3DCamera.

Definition at line 225 of file CGenericSensor.h.

void mrpt::hwdrivers::CHokuyoURG::setReducedFOV ( const double fov ) [inline]

If called (before calling "turnOn"), the field of view of the laser is reduced to the given range (in radians), discarding the rest of measures.

Call with "0" to disable this reduction again (the default).

Definition at line 204 of file CHokuyoURG.h.

void mrpt::hwdrivers::CGenericSensor::setSensorLabel ( const std::string & sensorLabel ) [inline, inherited]

Definition at line 103 of file CGenericSensor.h.

void mrpt::hwdrivers::CHokuyoURG::setSerialPort ( const std::string & port_name ) [inline]

If set to non-empty, the serial port will be attempted to be opened automatically when this class is first used to request data from the laser.

Definition at line 193 of file CHokuyoURG.h.

void mrpt::hwdrivers::CHokuyoURG::setVerbose ( bool enable = true ) [inline]

Definition at line 211 of file CHokuyoURG.h.

bool mrpt::hwdrivers::CHokuyoURG::startScanningMode ( ) [private]

Start the scanning mode, using parameters stored in the object (loaded from the .ini file) After this command the device will start to send scans until "switchLaserOff" is called.

Returns:: false on any error

bool mrpt::hwdrivers::CHokuyoURG::switchLaserOff ( ) [private]

Switchs the laser off.

Returns:: false on any error

bool mrpt::hwdrivers::CHokuyoURG::switchLaserOn ( ) [private]

Switchs the laser on.

Returns:: false on any error

bool mrpt::hwdrivers::CHokuyoURG::turnOff ( ) [virtual]

Disables the scanning mode (this can be used to turn the device in low energy mode, if available)

Returns:: If everything works "true", or "false" if there is any error.

Implements mrpt::hwdrivers::C2DRangeFinderAbstract.

bool mrpt::hwdrivers::CHokuyoURG::turnOn ( ) [virtual]

Enables the scanning mode (which may depend on the specific laser device); this must be called before asking for observations to assure that the protocol has been initializated.