lms400_cola.h
00001 /*
00002  *  Player - One Hell of a Robot Server
00003  *  Copyright (C) 2007
00004  *     Nico Blodow and Radu Bogdan Rusu
00005  *
00006  *
00007  *  This library is free software; you can redistribute it and/or
00008  *  modify it under the terms of the GNU Lesser General Public
00009  *  License as published by the Free Software Foundation; either
00010  *  version 2.1 of the License, or (at your option) any later version.
00011  *
00012  *  This library is distributed in the hope that it will be useful,
00013  *  but WITHOUT ANY WARRANTY; without even the implied warranty of
00014  *  MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00015  *  Lesser General Public License for more details.
00016  *
00017  *  You should have received a copy of the GNU Lesser General Public
00018  *  License along with this library; if not, write to the Free Software
00019  *  Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
00020  */
00021 
00022 /*
00023  Desc: Driver for the SICK LMS400 unit
00024  Author: Nico Blodow and Radu Bogdan Rusu
00025  Date: 7 Feb 2007
00026  CVS: $Id: lms400_cola.h 8135 2009-07-25 08:19:31Z thjc $
00027 */
00028 
00029 #include "config.h"
00030 
00031 #include <netdb.h>
00032 #include <sys/types.h>
00033 #include <vector>
00034 #include <netinet/in.h>
00035 #include <libplayerinterface/player.h>
00036 #include <iostream>
00037 
00038 #define BUF_SIZE 1024
00039 
00041 typedef struct
00042 {
00043   unsigned char* string;
00044   int length;
00045 } MeasurementQueueElement_t;
00046 
00048 typedef struct
00049 {
00050   uint16_t Format;
00051   uint16_t DistanceScaling;
00052   int32_t  StartingAngle;
00053   uint16_t AngularStepWidth;
00054   uint16_t NumberMeasuredValues;
00055   uint16_t ScanningFrequency;
00056   uint16_t RemissionScaling;
00057   uint16_t RemissionStartValue;
00058   uint16_t RemissionEndValue;
00059 } MeasurementHeader_t;
00060 
00061 
00063 class lms400_cola
00064 {
00065   public:
00066     lms400_cola (const char* host, int port, int debug_mode);
00067 
00068     // Creates socket, connects
00069     int Connect ();
00070     int Disconnect ();
00071 
00072     // Configuration parameters
00073     int SetAngularResolution (const char* password, float ang_res, float angle_start, float angle_range);
00074     int SetScanningFrequency (const char* password, float freq, float angle_start, float angle_range);
00075     int SetResolutionAndFrequency (float freq, float ang_res, float angle_start, float angle_range);
00076 
00077     int StartMeasurement (bool intensity = true);
00078     player_laser_data ReadMeasurement  ();
00079     int StopMeasurement  ();
00080 
00081     int SetUserLevel  (int8_t userlevel, const char* password);
00082     int GetMACAddress (char** macadress);
00083 
00084     int SetIP         (char* ip);
00085     int SetGateway    (char* gw);
00086     int SetNetmask    (char* mask);
00087     int SetPort       (uint16_t port);
00088 
00089     int ResetDevice            ();
00090     int TerminateConfiguration ();
00091 
00092     int SendCommand   (const char* cmd);
00093     int ReadResult    ();
00094     // for "Variables", Commands that only reply with one Answer message
00095     int ReadAnswer    ();
00096     // for "Procedures", Commands that reply with a Confirmation message and an Answer message
00097     int ReadConfirmationAndAnswer ();
00098 
00099     int EnableRIS (int onoff);
00100     player_laser_config GetConfiguration ();
00101     int SetMeanFilterParameters (int num_scans);
00102     int SetRangeFilterParameters (float *ranges);
00103     int EnableFilters (int filter_mask);
00104 
00105     // turns a string holding an ip address into long
00106     unsigned char* ParseIP (char* ip);
00107 
00108   private:
00109     // assembles STX's, length field, message, checksum ready to be sent. Cool.
00110     int assemblecommand (unsigned char* command, int len);
00111 
00112     const char* hostname;
00113     int sockfd, portno, n;
00114     struct sockaddr_in serv_addr;
00115 #if HAVE_GETADDRINFO
00116     struct addrinfo *addr_ptr;
00117 #else
00118     struct hostent *server;
00119 #endif
00120 
00121     // Internal Parameters:
00122     int verbose;
00123     int ExtendedRIS;
00124     int MeanFilterNumScans;
00125     float RangeFilterTopLimit;
00126     float RangeFilterBottomLimit;
00127     int FilterMask;
00128     player_laser_config Configuration;
00129 
00130     // for reading:
00131     unsigned char buffer[4096];
00132     unsigned int bufferlength;
00133 
00134     // for sending:
00135     unsigned char command[BUF_SIZE];
00136     int commandlength;
00137     std::vector<MeasurementQueueElement_t>* MeasurementQueue;
00138 };

Last updated 12 September 2005 21:38:45