Detailed Description

This "software driver" implements the communication protocol for interfacing a SICK LMS 2XX laser scanners through a standard RS232 serial port (or a USB2SERIAL converter).

The serial port is opened upon the first call to "doProcess" or "initialize", so you must call "loadConfig" before this, or manually call "setSerialPort". Another alternative is to call the base class method C2DRangeFinderAbstract::bindIO, but the "setSerialPort" interface is probably much simpler to use.

For an example of usage see the example in "samples/SICK_laser_serial_test". See also the example configuration file for rawlog-grabber in "share/mrpt/config_files/rawlog-grabber".

  PARAMETERS IN THE ".INI"-LIKE CONFIGURATION STRINGS:
 -------------------------------------------------------
   [supplied_section_name]
   COM_port_WIN = COM1   // Serial port to connect to
   COM_port_LIN = ttyS0

   COM_baudRate = 38400 // Possible values: 9600 (default), 38400, 5000000
   mm_mode      = 1/0   // 1: millimeter mode, 0:centimeter mode (Default=0)
   FOV          = 180   // Field of view: 100 or 180 degrees (Default=180)
   resolution   =  50   // Scanning resolution, in units of 1/100 degree. Valid values: 25,50,100 (Default=50)


   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

See also:: C2DRangeFinderAbstract

#include <mrpt/hwdrivers/CSickLaserSerial.h>

Inheritance diagram for mrpt::hwdrivers::CSickLaserSerial:

[legend]

List of all members.

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

CSickLaserSerial ()

Constructor.

virtual ~CSickLaserSerial ()

Destructor.

void setSerialPort (const std::string &port)

Changes the serial port to connect to (call prior to 'doProcess'), for example "COM1" or "ttyS0".

std::string getSerialPort () const

void setBaudRate (int baud)

Changes the serial port baud rate (call prior to 'doProcess'); valid values are 9600,38400 and 500000.

int getBaudRate () const

void setMillimeterMode (bool mm_mode=true)

Enables/Disables the millimeter mode, with a greater accuracy but a shorter range (default=false) (call prior to 'doProcess') This is not needed if the configuration is loaded with "loadConfig".

void setScanFOV (int fov_degrees)

Set the scanning field of view - possible values are 100 or 180 (default) (call prior to 'doProcess') This is not needed if the configuration is loaded with "loadConfig".

int getScanFOV () const

void setScanResolution (int res_1_100th_degree)

Set the scanning resolution, in units of 1/100 degree - Possible values are 25, 50 and 100, for 0.25, 0.5 (default) and 1 deg.

int getScanResolution () const

unsigned int getCurrentConnectTry () const

If performing several tries in ::initialize(), this is the current try loop number.

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.

void initialize ()

Set-up communication with the laser.

bool turnOn ()

Enables the scanning mode (in this class this has no effect).

bool turnOff ()

Disables the scanning mode (in this class this has no effect).

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

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::CSickLaserSerial 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

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 tryToOpenComms (std::string *err_msg=NULL)

Tries to open the com port and setup all the LMS protocol. Returns true if OK or already open.

bool waitContinuousSampleFrame (std::vector< float > &ranges, unsigned char &LMS_status, bool &is_mm_mode)

bool LMS_setupSerialComms ()

Assures laser is connected and operating at 38400, in its case returns true.

bool LMS_setupBaudrate (int baud)

Send a command to change the LMS comms baudrate, return true if ACK is OK. baud can be: 9600, 19200, 38400, 500000.

bool LMS_statusQuery ()

Send a status query and wait for the answer. Return true on OK.

bool LMS_waitACK (uint16_t timeout_ms)

Returns false if timeout.

bool LMS_waitIncomingFrame (uint16_t timeout)

Returns false if timeout.

bool LMS_sendMeasuringMode_cm_mm ()

Returns false on error.

bool LMS_startContinuousMode ()

bool LMS_endContinuousMode ()

bool SendCommandToSICK (const uint8_t *cmd, const uint16_t cmd_len)

Send header+command-data+crc and waits for ACK. Return false on error.

Private Attributes

bool m_mm_mode

int m_scans_FOV

100 or 180 deg

int m_scans_res

1/100th of deg: 100, 50 or 25

poses::TPose3D

m_sensorPose

The sensor 6D pose:

uint8_t m_received_frame_buffer [2000]

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.

CSerialPort * m_mySerialPort

Will be !=NULL only if I created it, so I must destroy it at the end.

int m_com_baudRate

Baudrate: 9600, 38400, 500000.

unsigned int m_nTries_connect

Default = 1.

unsigned int m_nTries_current

Static Private Attributes

static int CRC16_GEN_POL

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::CSickLaserSerial::CSickLaserSerial ( )

Constructor.

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

Destructor.

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.

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

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.

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

Main method for a CGenericSensor.

Implements mrpt::hwdrivers::CGenericSensor.

Reimplemented in mrpt::hwdrivers::CLMS100Eth.

void mrpt::hwdrivers::CSickLaserSerial::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.

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

int mrpt::hwdrivers::CSickLaserSerial::getBaudRate ( ) const [inline]

See also:: setBaudRate

Definition at line 138 of file CSickLaserSerial.h.

unsigned int mrpt::hwdrivers::CSickLaserSerial::getCurrentConnectTry ( ) const [inline]

If performing several tries in ::initialize(), this is the current try loop number.

Definition at line 159 of file CSickLaserSerial.h.

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]

int mrpt::hwdrivers::CSickLaserSerial::getScanFOV ( ) const [inline]

Definition at line 150 of file CSickLaserSerial.h.

int mrpt::hwdrivers::CSickLaserSerial::getScanResolution ( ) const [inline]

Definition at line 156 of file CSickLaserSerial.h.

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

Definition at line 102 of file CGenericSensor.h.

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

See also:: setSerialPort

Definition at line 129 of file CSickLaserSerial.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::CSickLaserSerial::initialize ( ) [virtual]

Set-up communication with the laser.

Called automatically by rawlog-grabber. If used manually, call after "loadConfig" and before "doProcess".

In this class this method does nothing, since the communications are setup at the first try from "doProcess" or "doProcessSimple".

Reimplemented from mrpt::hwdrivers::CGenericSensor.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_endContinuousMode ( ) [private]

bool mrpt::hwdrivers::CSickLaserSerial::LMS_sendMeasuringMode_cm_mm ( ) [private]

Returns false on error.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_setupBaudrate ( int baud ) [private]

Send a command to change the LMS comms baudrate, return true if ACK is OK. baud can be: 9600, 19200, 38400, 500000.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_setupSerialComms ( ) [private]

Assures laser is connected and operating at 38400, in its case returns true.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_startContinuousMode ( ) [private]

bool mrpt::hwdrivers::CSickLaserSerial::LMS_statusQuery ( ) [private]

Send a status query and wait for the answer. Return true on OK.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_waitACK ( uint16_t timeout_ms ) [private]

Returns false if timeout.

bool mrpt::hwdrivers::CSickLaserSerial::LMS_waitIncomingFrame ( uint16_t timeout ) [private]

Returns false if timeout.

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::CSickLaserSerial::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::CSickLaserSerial 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().

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.

bool mrpt::hwdrivers::CSickLaserSerial::SendCommandToSICK	(	const uint8_t *	cmd,
		const uint16_t	cmd_len
	)		`[private]`

Send header+command-data+crc and waits for ACK. Return false on error.

void mrpt::hwdrivers::CSickLaserSerial::setBaudRate ( int baud ) [inline]

Changes the serial port baud rate (call prior to 'doProcess'); valid values are 9600,38400 and 500000.

This is not needed if the configuration is loaded with "loadConfig".

See also:: getBaudRate

Definition at line 134 of file CSickLaserSerial.h.

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.

void mrpt::hwdrivers::CSickLaserSerial::setMillimeterMode ( bool mm_mode = true ) [inline]

Enables/Disables the millimeter mode, with a greater accuracy but a shorter range (default=false) (call prior to 'doProcess') This is not needed if the configuration is loaded with "loadConfig".

Definition at line 144 of file CSickLaserSerial.h.

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::CSickLaserSerial::setScanFOV ( int fov_degrees ) [inline]

Set the scanning field of view - possible values are 100 or 180 (default) (call prior to 'doProcess') This is not needed if the configuration is loaded with "loadConfig".

Definition at line 149 of file CSickLaserSerial.h.

void mrpt::hwdrivers::CSickLaserSerial::setScanResolution ( int res_1_100th_degree ) [inline]

Set the scanning resolution, in units of 1/100 degree - Possible values are 25, 50 and 100, for 0.25, 0.5 (default) and 1 deg.

(call prior to 'doProcess') This is not needed if the configuration is loaded with "loadConfig".

Definition at line 155 of file CSickLaserSerial.h.

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

Definition at line 103 of file CGenericSensor.h.

void mrpt::hwdrivers::CSickLaserSerial::setSerialPort ( const std::string & port ) [inline]

Changes the serial port to connect to (call prior to 'doProcess'), for example "COM1" or "ttyS0".

This is not needed if the configuration is loaded with "loadConfig".

Definition at line 126 of file CSickLaserSerial.h.

bool mrpt::hwdrivers::CSickLaserSerial::tryToOpenComms ( std::string * err_msg = NULL ) [private]

Tries to open the com port and setup all the LMS protocol. Returns true if OK or already open.

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

Disables the scanning mode (in this class this has no effect).

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

Implements mrpt::hwdrivers::C2DRangeFinderAbstract.

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

Enables the scanning mode (in this class this has no effect).

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

Implements mrpt::hwdrivers::C2DRangeFinderAbstract.

bool mrpt::hwdrivers::CSickLaserSerial::waitContinuousSampleFrame	(	std::vector< float > &	ranges,
		unsigned char &	LMS_status,
		bool &	is_mm_mode
	)		`[private]`