lms400_cola.h
1 /*
2  * Player - One Hell of a Robot Server
3  * Copyright (C) 2007
4  * Nico Blodow and Radu Bogdan Rusu
5  *
6  *
7  * This library is free software; you can redistribute it and/or
8  * modify it under the terms of the GNU Lesser General Public
9  * License as published by the Free Software Foundation; either
10  * version 2.1 of the License, or (at your option) any later version.
11  *
12  * This library is distributed in the hope that it will be useful,
13  * but WITHOUT ANY WARRANTY; without even the implied warranty of
14  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
15  * Lesser General Public License for more details.
16  *
17  * You should have received a copy of the GNU Lesser General Public
18  * License along with this library; if not, write to the Free Software
19  * Foundation, Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301 USA
20  */
21 
22 /*
23  Desc: Driver for the SICK LMS400 unit
24  Author: Nico Blodow and Radu Bogdan Rusu
25  Date: 7 Feb 2007
26  CVS: $Id$
27 */
28 
29 #include "config.h"
30 
31 #include <netdb.h>
32 #include <sys/types.h>
33 #include <vector>
34 #include <netinet/in.h>
35 #include <libplayerinterface/player.h>
36 #include <iostream>
37 
38 #define BUF_SIZE 1024
39 
41 typedef struct
42 {
43  unsigned char* string;
44  int length;
46 
48 typedef struct
49 {
50  uint16_t Format;
51  uint16_t DistanceScaling;
52  int32_t StartingAngle;
53  uint16_t AngularStepWidth;
54  uint16_t NumberMeasuredValues;
55  uint16_t ScanningFrequency;
56  uint16_t RemissionScaling;
57  uint16_t RemissionStartValue;
58  uint16_t RemissionEndValue;
60 
61 
64 {
65  public:
66  lms400_cola (const char* host, int port, int debug_mode);
67 
68  // Creates socket, connects
69  int Connect ();
70  int Disconnect ();
71 
72  // Configuration parameters
73  int SetAngularResolution (const char* password, float ang_res, float angle_start, float angle_range);
74  int SetScanningFrequency (const char* password, float freq, float angle_start, float angle_range);
75  int SetResolutionAndFrequency (float freq, float ang_res, float angle_start, float angle_range);
76 
77  int StartMeasurement (bool intensity = true);
78  player_laser_data ReadMeasurement ();
79  int StopMeasurement ();
80 
81  int SetUserLevel (int8_t userlevel, const char* password);
82  int GetMACAddress (char** macadress);
83 
84  int SetIP (char* ip);
85  int SetGateway (char* gw);
86  int SetNetmask (char* mask);
87  int SetPort (uint16_t port);
88 
89  int ResetDevice ();
90  int TerminateConfiguration ();
91 
92  int SendCommand (const char* cmd);
93  int ReadResult ();
94  // for "Variables", Commands that only reply with one Answer message
95  int ReadAnswer ();
96  // for "Procedures", Commands that reply with a Confirmation message and an Answer message
97  int ReadConfirmationAndAnswer ();
98 
99  int EnableRIS (int onoff);
100  player_laser_config GetConfiguration ();
101  int SetMeanFilterParameters (int num_scans);
102  int SetRangeFilterParameters (float *ranges);
103  int EnableFilters (int filter_mask);
104 
105  // turns a string holding an ip address into long
106  unsigned char* ParseIP (char* ip);
107 
108  private:
109  // assembles STX's, length field, message, checksum ready to be sent. Cool.
110  int assemblecommand (unsigned char* command, int len);
111 
112  const char* hostname;
113  int sockfd, portno, n;
114  struct sockaddr_in serv_addr;
115 #if HAVE_GETADDRINFO
116  struct addrinfo *addr_ptr;
117 #else
118  struct hostent *server;
119 #endif
120 
121  // Internal Parameters:
122  int verbose;
123  int ExtendedRIS;
124  int MeanFilterNumScans;
125  float RangeFilterTopLimit;
126  float RangeFilterBottomLimit;
127  int FilterMask;
128  player_laser_config Configuration;
129 
130  // for reading:
131  unsigned char buffer[4096];
132  unsigned int bufferlength;
133 
134  // for sending:
135  unsigned char command[BUF_SIZE];
136  int commandlength;
137  std::vector<MeasurementQueueElement_t>* MeasurementQueue;
138 };
T min(T a, T b)
Return the minimum of a, b.
Definition: utility.h:113
uint32_t maxqual
Maximum value for quality.
Definition: player_interfaces.h:2262
uint8_t state
FALSE for off, TRUE for on.
Definition: player_interfaces.h:667
position 2d velocity command
Definition: player_interfaces.h:617
uint32_t blobs_count
The number of blobs.
Definition: player_interfaces.h:1103
Request/reply: Get/set scan properties.
Definition: player_interfaces.h:961
Data: detected blobs (PLAYER_BLOBFINDER_DATA_BLOBS)
Definition: player_interfaces.h:1096
#define PLAYER_MSG3(level, msg, a, b, c)
Error message macros.
Definition: error.h:108
uint32_t maxlevel
Maximum value for level.
Definition: player_interfaces.h:2264
player_pose2d_t vel
translational velocities [m/s,m/s,rad/s] (x, y, yaw)
Definition: player_interfaces.h:620
#define PLAYER_LASER_DATA_SCAN
Data subtype: scan.
Definition: player_interfaces.h:845
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
float pan
Pan [rad].
Definition: player_interfaces.h:1231
#define PLAYER_WIFI_MODE_AUTO
driver decides the mode
Definition: player_interfaces.h:2202
#define PLAYER_WIFI_MODE_MASTER
access point, master mode
Definition: player_interfaces.h:2208
uint32_t right
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1084
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
player_fiducial_item_t * fiducials
List of detected fiducials.
Definition: player_interfaces.h:1705
#define PLAYER_PTZ_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:1208
#define PLAYER_WIFI_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2188
int AddInterface(player_devaddr_t addr)
Add an interface.
double px
X [m].
Definition: player.h:231
uint32_t qual_type
Indicates type of link quality info we have.
Definition: player_interfaces.h:2260
#define PLAYER_WIFI_QUAL_DBM
link quality is in dBm
Definition: player_interfaces.h:2193
#define PLAYER_WIFI_MODE_ADHOC
ad hoc mode
Definition: player_interfaces.h:2204
uint32_t height
The image dimensions.
Definition: player_interfaces.h:1101
uint32_t left
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1082
uint32_t links_count
length of said list
Definition: player_interfaces.h:2250
Generic message header.
Definition: player.h:161
#define PLAYER_WIFI_QUAL_UNKNOWN
link quality is unknown
Definition: player_interfaces.h:2197
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:888
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
#define PLAYER_CAMERA_REQ_GET_IMAGE
Request/reply subtype: get current image frame.
Definition: player_interfaces.h:2931
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
Data: state (PLAYER_PTZ_DATA_STATE)
Definition: player_interfaces.h:1228
#define PLAYER_CAMERA_FORMAT_RGB888
Image format : 24-bit color (8 bits R, 8 bits G, 8 bits B).
Definition: player_interfaces.h:2942
float tilt
Desired tilt angle [rad].
Definition: player_interfaces.h:1253
Command: state (PLAYER_PTZ_CMD_STATE)
Definition: player_interfaces.h:1248
uint32_t bottom
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1088
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:74
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
#define PLAYER_POSITION2D_REQ_MOTOR_POWER
Request/reply: Motor power.
Definition: player_interfaces.h:496
position2d power config
Definition: player_interfaces.h:664
double px
X [m].
Definition: player.h:220
float zoom
Desired field of view [rad].
Definition: player_interfaces.h:1255
#define PLAYER_BLOBFINDER_DATA_BLOBS
Structure describing a single blob.
Definition: player_interfaces.h:1055
float zoom
Field of view [rad].
Definition: player_interfaces.h:1235
#define PLAYER_POSITION2D_CMD_VEL
Command: velocity (PLAYER_POSITION2D_CMD_VEL)
Definition: player_interfaces.h:581
double pyaw
yaw [rad]
Definition: player.h:241
double ReadTupleFloat(int section, const char *name, int index, double value)
Read a float (double) from a tuple field.
virtual void Main(void)=0
Main method for driver thread.
uint32_t maxnoise
Maximum value for noise.
Definition: player_interfaces.h:2266
Definition: lms400_cola.h:63
#define PLAYER_GRIPPER_CMD_STOP
Command: Stop (PLAYER_GRIPPER_CMD_STOP)
Definition: player_interfaces.h:383
int ReadInt(int section, const char *name, int value)
Read an integer value.
double ReadLength(int section, const char *name, double value)
Read a length (includes unit conversion, if any).
uint32_t y
The blob centroid [pixels].
Definition: player_interfaces.h:1080
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
double py
Y [m].
Definition: player.h:222
#define PLAYER_ERROR2(msg, a, b)
Error message macros.
Definition: error.h:83
virtual void Update()
Update non-threaded drivers.
Definition: driver.h:423
uint32_t area
The blob area [pixels].
Definition: player_interfaces.h:1076
#define PLAYER_JOYSTICK_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:3332
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
#define PLAYER_CAMERA_DATA_STATE
Data subtype: state.
Definition: player_interfaces.h:2922
uint32_t id
A unique, increasing, ID for the scan.
Definition: player_interfaces.h:902
#define PLAYER_GRIPPER_CMD_OPEN
Command: Open (PLAYER_GRIPPER_CMD_OPEN)
Definition: player_interfaces.h:373
uint32_t id
Blob id.
Definition: player_interfaces.h:1071
float * ranges
Range readings [m].
Definition: player_interfaces.h:896
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
#define PLAYER_WIFI_MODE_REPEAT
repeater mode
Definition: player_interfaces.h:2210
uint32_t mode
operating mode of device
Definition: player_interfaces.h:2258
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
#define PLAYER_FIDUCIAL_REQ_GET_GEOM
Info on a single detected fiducial.
Definition: player_interfaces.h:1666
char ap[32]
MAC address of current access point/cell.
Definition: player_interfaces.h:2268
uint32_t bitrate
current bitrate of device
Definition: player_interfaces.h:2256
#define PLAYER_WIFI_MODE_INFRA
infrastructure mode (multi cell network, roaming)
Definition: player_interfaces.h:2206
#define PLAYER_WIFI_QUAL_REL
link quality is relative
Definition: player_interfaces.h:2195
Info on a single detected fiducial.
Definition: player_interfaces.h:1685
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Data: state (PLAYER_CAMERA_DATA_STATE)
Definition: player_interfaces.h:2950
uint32_t top
Bounding box for the blob [pixels].
Definition: player_interfaces.h:1086
Definition: lms400_cola.h:48
float pan
Desired pan angle [rad].
Definition: player_interfaces.h:1251
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
float resolution
Scan resolution [rad].
Definition: player_interfaces.h:968
Class for loading configuration file information.
Definition: configfile.h:196
int ReadTupleInt(int section, const char *name, int index, int value)
Read an integer from a tuple field.
virtual int Setup()
Initialize the driver.
Definition: driver.h:386
#define PLAYER_CAPABILITIES_REQ
Capability request message type.
Definition: player.h:397
A device address.
Definition: player.h:145
An autopointer for the message queue.
Definition: message.h:73
double py
Y [m].
Definition: player.h:233
uint32_t fiducials_count
The number of detected fiducials.
Definition: player_interfaces.h:1703
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:964
Data: detected fiducials (PLAYER_FIDUCIAL_DATA_SCAN)
Definition: player_interfaces.h:1700
Definition: lms400_cola.h:41
void SetError(int code)
Set/reset error code.
Definition: driver.h:145
position2d data
Definition: player_interfaces.h:606
player_wifi_link_t * links
A list of links.
Definition: player_interfaces.h:2252
#define PLAYER_ERROR1(msg, a)
Error message macros.
Definition: error.h:82
uint32_t width
The image dimensions.
Definition: player_interfaces.h:1099
#define PLAYER_CAMERA_COMPRESS_RAW
Compression method: raw.
Definition: player_interfaces.h:2945
Data: state (PLAYER_JOYSTICK_DATA_STATE)
Definition: player_interfaces.h:3341
#define PLAYER_ERROR(msg)
Error message macros.
Definition: error.h:81
float min_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:886
#define PLAYER_FIDUCIAL_DATA_SCAN
Info on a single detected fiducial.
Definition: player_interfaces.h:1663
Base class for drivers which oeprate with a thread.
Definition: driver.h:552
#define PLAYER_GRIPPER_CMD_STORE
Command: Store (PLAYER_GRIPPER_CMD_STORE)
Definition: player_interfaces.h:388
#define PLAYER_PTZ_CMD_STATE
Command subtype: state.
Definition: player_interfaces.h:1214
#define PLAYER_GRIPPER_CMD_CLOSE
Command: Close (PLAYER_GRIPPER_CMD_CLOSE)
Definition: player_interfaces.h:378
Data: state (PLAYER_WIFI_DATA_STATE)
Definition: player_interfaces.h:2247
#define PLAYER_POSITION2D_DATA_STATE
Data: state (PLAYER_POSITION2D_DATA_STATE)
Definition: player_interfaces.h:568
float max_range
Maximum range [m].
Definition: player_interfaces.h:892
player_blobfinder_blob_t * blobs
The list of blobs.
Definition: player_interfaces.h:1105
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
float max_angle
Start and end angles for the laser scan [rad].
Definition: player_interfaces.h:966
#define PLAYER_WIFI_REQ_MAC
Request/reply subtype:
Definition: player_interfaces.h:2176
player_pose3d_t pose
Fiducial pose relative to the detector.
Definition: player_interfaces.h:1691
Reference-counted message objects.
Definition: message.h:132
Request/reply:
Definition: player_interfaces.h:2272
int32_t id
The fiducial id.
Definition: player_interfaces.h:1689
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_WIFI_MODE_UNKNOWN
unknown operating mode
Definition: player_interfaces.h:2200
#define PLAYER_GRIPPER_CMD_RETRIEVE
Command: Retrieve (PLAYER_GRIPPER_CMD_RETRIEVE)
Definition: player_interfaces.h:394
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:393
double pa
yaw [rad]
Definition: player.h:224
Base class for all drivers.
Definition: driver.h:108
uint32_t throughput
mysterious throughput calculated by driver
Definition: player_interfaces.h:2254
Structure describing a single blob.
Definition: player_interfaces.h:1068
uint32_t intensity_count
Number of intensity readings.
Definition: player_interfaces.h:898
#define PLAYER_WIFI_MODE_SECOND
secondary/backup repeater
Definition: player_interfaces.h:2212
uint32_t x
The blob centroid [pixels].
Definition: player_interfaces.h:1078
uint8_t state
Motor state (FALSE is either off or locked, depending on the driver).
Definition: player_interfaces.h:622
float scanning_frequency
Scanning frequency [Hz].
Definition: player_interfaces.h:976
float resolution
Angular resolution [rad].
Definition: player_interfaces.h:890
uint32_t ranges_count
Number of range readings.
Definition: player_interfaces.h:894
Data: scan (PLAYER_LASER_DATA_SCAN)
Definition: player_interfaces.h:883
#define PLAYER_MSGQUEUE_DEFAULT_MAXLEN
Default maximum length for a message queue.
Definition: player.h:76
T max(T a, T b)
Return the maximum of a, b.
Definition: utility.h:126
uint8_t * intensity
Intensity readings.
Definition: player_interfaces.h:900