data_scans.cc

Go to the documentation of this file.

/*
 *  Copyright (C) 2007 Austin Robot Technology, Patrick Beeson
 *  Copyright (C) 2009, 2010 Austin Robot Technology, Jack O'Quin
 *
 *  License: Modified BSD Software License Agreement
 *
 *  $Id: data_scans.cc 779 2010-11-13 19:41:40Z jack.oquin $
 */

#include <fstream>

#include <ros/ros.h>
#include <angles/angles.h>

#include <velodyne/data_scans.h>

namespace Velodyne
{
  //
  // DataScans class implementation
  //

  DataScans::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_.reserve(SCANS_PER_REV);
  }

  void DataScans::packet2scans(const raw_packet_t *raw, laserscan_t *scans)
  {
    int index = 0;                      // current scans entry
    uint16_t revolution = raw->revolution; // current revolution (mod 65536)

    for (int i = 0; i < BLOCKS_PER_PACKET; i++)
      {
        int bank_origin = 32;
        correction_angles *corrections = upper_;
        if (raw->blocks[i].header == LOWER_BANK)
          {
            bank_origin = 0;
            corrections = lower_;
          }

        float rotation = angles::from_degrees(raw->blocks[i].rotation
                                              * ROTATION_RESOLUTION);

        for (int j = 0, k = 0; j < SCANS_PER_BLOCK; j++, k += RAW_SCAN_SIZE)
          {
            scans[index].laser_number = j + bank_origin;

            //if(!corrections[j].enabled) 
            //  do what???

            // beware: the Velodyne turns clockwise
            scans[index].heading = 
              angles::normalize_angle(-(rotation - corrections[j].rotational));
            scans[index].pitch   = corrections[j].vertical;
      
            union two_bytes tmp;
            tmp.bytes[0] = raw->blocks[i].data[k];
            tmp.bytes[1] = raw->blocks[i].data[k+1];

            // convert range to meters and apply quadratic correction
            scans[index].range = tmp.uint * DISTANCE_RESOLUTION;
            scans[index].range =
              (corrections[j].offset1 * scans[index].range * scans[index].range
               + corrections[j].offset2 * scans[index].range
               + corrections[j].offset3);
      
            scans[index].intensity = raw->blocks[i].data[k+2];
            scans[index].revolution = revolution;

            ++index;
          }
      }

    ROS_ASSERT(index == SCANS_PER_PACKET);
  }

  void DataScans::processPacket(const velodyne_msgs::VelodynePacket *pkt,
                                const std::string &frame_id)
  {
    // unpack scans from the raw packet
    scans_.resize(SCANS_PER_PACKET);
    packet2scans((raw_packet_t *) &pkt->data[0], &scans_[0]);

    if (!ros::ok())                     // check for ROS shutdown
      return;

    // invoke the subscribed scans callback, if any
    if (cb_)
      cb_(scans_, pkt->stamp, frame_id);
  }

  int DataScans::print(void)
  {
    if (uninitialized_)                 // has setup() run?
      return EBADF;

    if (ofp_ == NULL)                   // no output desired?
      return 0;

    for (unsigned i = 0; i < scans_.size(); i++)
      {
        int rc = fprintf(ofp_, "%+08.6f %+08.6f %+011.6f %03u %02u %05u\n",
                         scans_[i].heading,
                         scans_[i].pitch,
                         scans_[i].range,
                         scans_[i].intensity,
                         scans_[i].laser_number,
                         scans_[i].revolution);
        if (rc < 0)
          return errno;

        // this loop runs for a long time, so check if a shutdown
        // signal was received
        if (!ros::ok())                 // ROS shutting down?
          return EINTR;                 // interrupted
      }

    return 0;
  }

} // namespace velodyne

---

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