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:91
#define PLAYER_MSG3(level, msg, a, b, c)
Definition: error.h:108
static bool MatchMessage(player_msghdr_t *hdr, int type, int subtype, player_devaddr_t addr)
Helper for message processing.
Definition: message.h:159
double ReadFloat(int section, const char *name, double value)
Read a floating point (double) value.
int AddInterface(player_devaddr_t addr)
Add an interface.
Generic message header.
Definition: player.h:162
virtual int MainSetup(void)
Sets up the resources needed by the driver thread.
Definition: driver.h:658
virtual void MainQuit(void)
Cleanup method for driver thread (called when main exits)
Definition: driver.h:664
Encapsulates a device (i.e., a driver bound to an interface)
Definition: device.h:75
const char * ReadString(int section, const char *name, const char *value)
Read a string value.
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.
Definition: lms400_cola.h:64
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).
#define PLAYER_MSGTYPE_DATA
A data message.
Definition: player.h:95
#define PLAYER_ERROR2(msg, a, b)
Definition: error.h:83
virtual void Update()
Update non-threaded drivers.
Definition: driver.h:423
#define PLAYER_MSGTYPE_RESP_ACK
A positive response message.
Definition: player.h:112
virtual int ProcessMessage(QueuePointer &resp_queue, player_msghdr *hdr, void *data)
Message handler.
#define PLAYER_MSGTYPE_REQ
A request message.
Definition: player.h:106
int ReadDeviceAddr(player_devaddr_t *addr, int section, const char *name, int code, int index, const char *key)
Read a device id.
Definition: lms400_cola.h:49
int GetTupleCount(int section, const char *name)
Get the number of values in a tuple.
Class for loading configuration file information.
Definition: configfile.h:197
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:146
An autopointer for the message queue.
Definition: message.h:74
Definition: lms400_cola.h:42
void SetError(int code)
Set/reset error code.
Definition: driver.h:145
#define PLAYER_ERROR1(msg, a)
Definition: error.h:82
#define PLAYER_ERROR(msg)
Definition: error.h:81
Base class for drivers which oeprate with a thread.
Definition: driver.h:553
double timestamp
Time associated with message contents (seconds since epoch)
Definition: player.h:170
Reference-counted message objects.
Definition: message.h:133
#define PLAYER_WARN(msg)
Warning message macros.
Definition: error.h:89
#define PLAYER_MSGTYPE_CMD
A command message.
Definition: player.h:99
virtual int Shutdown()
Finalize the driver.
Definition: driver.h:393
Base class for all drivers.
Definition: driver.h:109
#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:104