data.h

Go to the documentation of this file.

/* -*- mode: C++ -*-
 *
 *  Copyright (C) 2007 Austin Robot Technology, Yaxin Liu, Patrick Beeson
 *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
 *
 *  License: Modified BSD Software License Agreement
 *
 *  $Id: data.h 819 2010-11-20 03:13:07Z jack.oquin $
 */

#ifndef __VELODYNE_DATA_H
#define __VELODYNE_DATA_H

#include <errno.h>
#include <stdint.h>
#include <string>

#include <ros/ros.h>
#include <velodyne_common/RawScan.h>
#include <boost/format.hpp>

namespace velodyne
{

  static const int SIZE_BLOCK = 100;
  static const int RAW_SCAN_SIZE = 3;
  static const int SCANS_PER_BLOCK = 32;
  static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);

  static const float ROTATION_RESOLUTION = 0.01f; 
  static const float ROTATION_MAX_UNITS = 36000; 
  static const float DISTANCE_MAX = 130.0f;        
  static const float DISTANCE_RESOLUTION = 0.002f; 
  static const float DISTANCE_MAX_UNITS = (DISTANCE_MAX
                                           / DISTANCE_RESOLUTION + 1.0);
  static const uint16_t UPPER_BANK = 0xeeff;
  static const uint16_t LOWER_BANK = 0xddff;

  typedef struct raw_block
  {
    uint16_t header;        
    uint16_t rotation;      
    uint8_t  data[BLOCK_DATA_SIZE];
  } raw_block_t;

  union two_bytes
  {
    uint16_t uint;
    uint8_t  bytes[2];
  };

  static const int PACKET_SIZE = 1206;
  static const int BLOCKS_PER_PACKET = 12;
  static const int PACKET_STATUS_SIZE = 4;
  static const int SCANS_PER_PACKET = (SCANS_PER_BLOCK * BLOCKS_PER_PACKET);
  static const int PACKETS_PER_REV = 260;
  static const int SCANS_PER_REV = (SCANS_PER_PACKET * PACKETS_PER_REV);

  typedef struct raw_packet
  {
    raw_block_t blocks[BLOCKS_PER_PACKET];
    uint16_t revolution;
    uint8_t status[PACKET_STATUS_SIZE]; 
  } raw_packet_t;

  struct correction_angles
  {
    float rotational;
    float vertical;
    float offset1, offset2, offset3;
    int   enabled;
  };

  typedef void (*raw_callback_t)(const raw_packet_t *raw, size_t npackets);

  class Data
  {
  public:
    Data(std::string ofile="",
         std::string anglesFile="")
      {
        ofile_ = ofile;
        anglesFile_ = anglesFile;

        ofp_ = NULL;
        rawCB_ = NULL;
        memset(&upper_, 0, sizeof(upper_));
        memset(&lower_, 0, sizeof(lower_));
      }

    virtual ~Data() {}

#if !ROS_VERSION_MINIMUM(1, 3, 0)

    virtual const roslib::Header *getMsgHeader(void) const
    {
      if (rawScan_)
        return &rawScan_->header;
      else
        return NULL;
    }
#endif

    void getMsgHeaderFields(ros::Time &stamp, std::string &frame_id) const
    {
      if (rawScan_)
        {
          stamp = rawScan_->header.stamp;
          frame_id = rawScan_->header.frame_id;
        }
    }

    virtual int getParams(void);

    virtual int print(void)
    {
      if (uninitialized_)
        return EBADF;                   // bad file descriptor
      return 0;
    }

    void processRawScan(const velodyne_common::RawScan::ConstPtr &raw_scan);

    virtual void processRaw(const raw_packet_t *raw, size_t npackets);

    virtual int setup(void);

    virtual void shutdown(void)
    {
      ros::shutdown();                  // break out of main ros::spin()
      uninitialized_ = true;
    }

    void subscribeRaw(raw_callback_t rawCB)
    {
      ROS_INFO("raw callback defined");
      rawCB_ = rawCB;
    }

  protected:

    std::string ofile_;                 
    std::string anglesFile_;            

    FILE *ofp_;                         
    raw_callback_t rawCB_;              
    bool uninitialized_;                

    velodyne_common::RawScan::ConstPtr rawScan_;

    correction_angles lower_[SCANS_PER_BLOCK];
    correction_angles upper_[SCANS_PER_BLOCK];
  };


  typedef struct laserscan
  {
    float range;                        
    float heading;                      
    float pitch;                        
    uint16_t revolution;
    uint8_t  laser_number;
    uint8_t  intensity;
  } laserscan_t;

  typedef void (*scans_callback_t)(const std::vector<laserscan_t> &scan);
  typedef boost::function<void(const std::vector<laserscan_t> &)> scanCallback;

  class DataScans: public Data
  {
  public:
    DataScans(std::string ofile="",
              std::string anglesFile=""):
      Data(ofile, anglesFile)
    {
      // reserve vector space before processing, we don't want to
      // reallocate in real time
      scans_.resize(SCANS_PER_REV);
      scansCB_ = NULL;
    }

    virtual int print(void);
    virtual void processRaw(const raw_packet_t *raw, size_t npackets);

    ros::Subscriber subscribe(ros::NodeHandle node,
                              const std::string &topic,
                              uint32_t queue_size,
                              const scanCallback callback,
                              const ros::TransportHints &transport_hints
                                      = ros::TransportHints())
    {
      ROS_INFO("generic scan callback defined");
      cb_ = callback;
      return node.subscribe(topic, queue_size,
                            &velodyne::Data::processRawScan,
                            (velodyne::Data *) this,
                            transport_hints);
    }

    virtual void subscribeScans(scans_callback_t scansCB)
    {
      ROS_INFO("scans callback defined");
      scansCB_ = scansCB;
    }

  protected:
    void packet2scans(const raw_packet_t *raw, laserscan_t *scans);

    // derived class data
    std::vector<laserscan_t> scans_;

  private:
    scans_callback_t scansCB_;          // deprecated
    scanCallback cb_;                   
  };


  typedef struct laserscan_xyz
  {
    float x, y, z;
    float heading;
    uint16_t revolution;
    uint8_t  laser_number;
    uint8_t  intensity;
  } laserscan_xyz_t;

  typedef void (*xyz_callback_t)(const std::vector<laserscan_xyz_t> &scan);
  typedef boost::function<void(const std::vector<laserscan_xyz_t> &)> xyzCallback;

  class DataXYZ: public DataScans
  {
  public:
    DataXYZ(std::string ofile="",
            std::string anglesFile=""):
      DataScans(ofile, anglesFile)
    {
      // reserve vector space before processing, we don't want to
      // reallocate in real time
      xyzScans_.resize(SCANS_PER_REV);
      xyzCB_ = NULL;
    }

    virtual int print(void);
    virtual void processRaw(const raw_packet_t *raw, size_t npackets);

    ros::Subscriber subscribe(ros::NodeHandle node,
                              const std::string &topic,
                              uint32_t queue_size,
                              const xyzCallback callback,
                              const ros::TransportHints &transport_hints
                                      = ros::TransportHints())
    {
      ROS_INFO("XYZ callback subscribed");
      cb_ = callback;
      return node.subscribe(topic, queue_size,
                            &velodyne::Data::processRawScan,
                            (velodyne::Data *) this,
                            transport_hints);
    }

    virtual void subscribeXYZ(xyz_callback_t xyzCB)
    {
      ROS_INFO("XYZ callback defined");
      xyzCB_ = xyzCB;
    }

  protected:
    void scan2xyz(const laserscan_t *scan, laserscan_xyz_t *point);

    // derived class data
    std::vector<laserscan_xyz_t> xyzScans_;

  private:
    xyz_callback_t xyzCB_;              // deprecated
    xyzCallback cb_;                    
  };


} // velodyne namespace

#endif // __VELODYNE_DATA_H

---

velodyne_common
Author(s): Jack O'Quin, Patrick Beeson, Michael Quinlan, Yaxin Liu
autogenerated on Fri Jan 11 10:05:54 2013