/build/rock-master-18.09-drivers-hokuyo-0.20160711/hokuyo.hh

Go to the documentation of this file.

#ifndef URG_H
#define URG_H

#include <map>
#include <string>
#include <iosfwd>
#include <sys/types.h>
#include <base/samples/LaserScan.hpp>
#include <iodrivers_base/Driver.hpp>

class URG : public iodrivers_base::Driver {
public:
  struct StatusCode;
  struct SimpleCommand;

  static const int MDMS_COMMAND_LENGTH   = 15;
  static const int MDMS_TIMESTAMP_OFFSET = MDMS_COMMAND_LENGTH + 5;
  static const int MDMS_DATA_OFFSET      = MDMS_TIMESTAMP_OFFSET + 6;

  struct DeviceInfo {
      DeviceInfo()
          : version(UNKNOWN), dMin(0), dMax(0), resolution(0), stepMin(0), stepMax(0), stepFront(0), motorSpeed(0) { }

      enum VERSIONS {
          URG04LX,
          UTM30LX,
          UST20LX,
          UNKNOWN
      };
      std::map<std::string, std::string> values;
      VERSIONS version;
      int dMin;       
      int dMax;       
      int resolution; 
      int stepMin;    
      int stepMax;    
      int stepFront;  
      float motorSpeed; 
  };

  enum ERROR_CODES {
    OK                  = 0,
    
    INTERNAL_COM_FAILED    = 2,
    DEVICE_BUFFER_OVERFLOW = 3,
    BAD_COMMAND         = 4,
    STRING_TOO_LONG     = 5,
    BAD_STRING          = 6,
    FIRMWARE_UPDATE     = 7,
    LASER_MALFUNCTION   = 8,
    HARDWARE_FAILURE    = 9,
    NON_APPLICABLE      = 10,
    
    BAD_REPLY                   = 100,
    DRIVER_BUFFER_OVERFLOW      = 121,
    PROVIDED_BUFFER_TOO_SMALL   = 122,
    BAD_STATE                   = 123,
    READ_FAILED                 = 124,
    BAD_RATE                    = 125,
    WRITE_FAILED                = 126,
    READ_TIMEOUT                = 127,
    NOT_SCIP2_CAPABLE           = 128,
    BAD_HOST_RATE               = 129,
    INCONSISTEN_RANGE_COUNT     = 130,
    WRITE_TIMEOUT               = 131,

    DUPLICATE                   = 132,

    UNKNOWN                     = 200,
    INTERNAL_ERROR              = 201,
    UNKNOWN_DEVICE_VERSION      = 202,

    END                         = 500
  };

private:
  static const unsigned int MAX_PACKET_SIZE = 8192;

  int         baudrate;
  char        m_last_status[3];
  ERROR_CODES m_error;
  DeviceInfo  m_info;

  int last_device_timestamp;
  base::Time device_time_offset;
  
  base::Time last_sample_time;

  static const int MAX_RANGE_READING = 60000;
  std::vector<float> remission_lookup;
  float normaliseRemission( int raw, int range );
  void initRemissionLookup();

  int sample_count;
  
  int readAnswer(char* buffer, size_t buffer_size, char const* expected_cmd, int timeout = 1000);

  int readAnswer(char* buffer, size_t buffer_size, char const** expected_cmds, int timeout = 1000);

  bool readInfo();
  bool simpleCommand(SimpleCommand const& cmd, int timeout = 1000);
  bool write(char const* string, int timeout = 1000);

  bool error(ERROR_CODES code);
  bool parseErrorCode(char const* code, StatusCode const* specific_codes);
  bool infoCommand(std::map<std::string, std::string>& result, char const* cmd, bool scip1 = false);
  bool timeCommand(int &device_timestamp, int timeout);

protected:
  int extractPacket(uint8_t const* buffer, size_t buffer_size) const;

public:
  URG();
  ~URG();

  void openURI(std::string const& filename);
  bool open(std::string const& filename);
  bool fullSerialReset();
  bool initCommunication(int timeout = 1000);
  bool measureCommunicationLatency(int timeout = 1000);
  void close();
  DeviceInfo getInfo() const { return m_info; };

  bool startAcquisition(int nScans, int startStep = -1, int endStep = -1, int scanInterval = 0, int clusterCount = 1, bool includeRemission = false );
  bool stopAcquisition();
  bool readRanges(base::samples::LaserScan& range, int timeout = -1);
  bool setBaudrate(int brate);

  int getPacketCounter();
  
  ERROR_CODES error() const { return m_error; }
  static char const* errorString(int error_code);
  char const* errorString() const { return errorString(m_error); }
};

std::ostream& operator << (std::ostream& io, URG::DeviceInfo info);

#endif