merge new interfaces from Piyush

fix unit test cases
issue error (not warning) if no calibration file

velodyne_pointcloud/include/velodyne_pointcloud/calibration.h

@@ -0,0 +1,73 @@
+/**
+ * \file  calibration.h 
+ *
+ * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
+ * Copyright (C) 2012, Austin Robot Technology, University of Texas at Austin
+ *
+ * License: Modified BSD License
+ *
+ * $ Id: 02/14/2012 11:25:34 AM piyushk $
+ */
+
+#ifndef CALIBRATION_CQUVIJWI
+#define CALIBRATION_CQUVIJWI
+
+#include <map>
+#include <string>
+
+namespace velodyne_pointcloud {
+
+  /** \brief correction values for a single laser
+   *
+   * Correction values for a single laser (as provided by db.xml from veleodyne). Includes 
+   * parameters for Velodyne HDL-64E S2.1
+   * http://velodynelidar.com/lidar/products/manual/63-HDL64E%20S2%20Manual_Rev%20D_2011_web.pdf
+   **/
+  struct LaserCorrection {
+
+    /** parameters in db.xml */
+    float rot_correction;
+    float vert_correction;
+    float dist_correction;
+    bool two_pt_correction_available;
+    float dist_correction_x;
+    float dist_correction_y;
+    float vert_offset_correction;
+    float horiz_offset_correction;
+    int max_intensity;
+    int min_intensity;
+    float focal_distance;
+    float focal_slope;
+
+    /** cached values calculated when the calibration file is read */
+    float cos_rot_correction;              ///< cached cosine of rot_correction
+    float sin_rot_correction;              ///< cached sine of rot_correction
+    float cos_vert_correction;             ///< cached cosine of vert_correction
+    float sin_vert_correction;             ///< cached sine of vert_correction
+  };
+
+  /** \brief Calibration class storing entire configuration for the Velodyne */
+  class Calibration {
+  public:
+    float pitch;
+    float roll;
+    std::map<int, LaserCorrection> laser_corrections;
+    bool initialized;
+
+    Calibration() : initialized(false) {}
+    Calibration(const std::string& calibration_file) {
+      read(calibration_file);
+    }
+    void read(const std::string& calibration_file);
+    void write(const std::string& calibration_file);
+    bool isInitialized() {
+      return initialized;
+    }
+  };
+
+} /* velodyne_pointcloud */
+
+
+#endif /* end of include guard: CALIBRATION_CQUVIJWI */
+
+

velodyne_pointcloud/include/velodyne_pointcloud/rawdata.h

@@ -29,6 +29,7 @@
 #include <pcl_ros/point_cloud.h>
 #include <velodyne_msgs/VelodyneScan.h>
 #include <velodyne_pointcloud/point_types.h>
+#include <velodyne_pointcloud/calibration.h>
 
 namespace velodyne_rawdata
 {
@@ -45,7 +46,7 @@ namespace velodyne_rawdata
   static const int BLOCK_DATA_SIZE = (SCANS_PER_BLOCK * RAW_SCAN_SIZE);
 
   static const float ROTATION_RESOLUTION = 0.01f; /**< degrees */
-  static const float ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */
+  static const uint16_t ROTATION_MAX_UNITS = 36000; /**< hundredths of degrees */
 
   /** According to Bruce Hall DISTANCE_MAX is 65.0, but we noticed
    *  valid packets with readings up to 130.0. */
@@ -107,17 +108,7 @@ namespace velodyne_rawdata
     uint8_t status[PACKET_STATUS_SIZE]; 
   } raw_packet_t;
 
-  /** \brief Correction angles for a specific HDL-64E device. */
-  struct correction_angles
-  {
-    float rotational;
-    float vertical;
-    float offset1, offset2, offset3;
-    float horzCorr, vertCorr;
-    int   enabled;
-  };
-
-  /** \brief Base Velodyne data class -- not used directly. */
+  /** \brief Velodyne data conversion class */
   class RawData
   {
   public:
@@ -150,12 +141,12 @@ namespace velodyne_rawdata
     } Config;
     Config config_;
 
-    /** correction angles indexed by laser within bank
-     *
-     * \todo combine them into a single array, lower followed by upper
+    /** 
+     * Calibration file
      */
-    correction_angles lower_[SCANS_PER_BLOCK];
-    correction_angles upper_[SCANS_PER_BLOCK];
+    velodyne_pointcloud::Calibration calibration_;
+    float sin_rot_table_[ROTATION_MAX_UNITS];
+    float cos_rot_table_[ROTATION_MAX_UNITS];
 
     /** in-line test whether a point is in range */
     bool pointInRange(float range)

velodyne_pointcloud/manifest.xml

@@ -17,6 +17,8 @@
   <depend package="velodyne_driver"/> <!-- launch files for testing -->
   <depend package="velodyne_msgs"/>
 
+  <rosdep name="yaml-cpp"/>
+
   <export>
     <cpp cflags="-I${prefix}/include"
          lflags="-L${prefix}/lib -Wl,-rpath,${prefix}/lib -lvelodyne_rawdata"/>

velodyne_pointcloud/src/lib/CMakeLists.txt

@@ -1 +1,5 @@
+rosbuild_add_library(velodyne_calibration calibration.cc)
+target_link_libraries(velodyne_calibration yaml-cpp)
+
 rosbuild_add_library(velodyne_rawdata rawdata.cc)
+target_link_libraries(velodyne_rawdata velodyne_calibration)

velodyne_pointcloud/src/lib/calibration.cc

@@ -0,0 +1,133 @@
+/**
+ * \file  calibration.cc
+ * \brief  
+ *
+ * \author  Piyush Khandelwal (piyushk@cs.utexas.edu)
+ * Copyright (C) 2012, Austin Robot Technology, The University of Texas at Austin
+ *
+ * License: Modified BSD License
+ *
+ * $ Id: 02/14/2012 11:36:36 AM piyushk $
+ */
+
+#include <iostream>
+#include <fstream>
+#include <string>
+#include <cmath>
+#include <yaml-cpp/yaml.h>
+
+#include <velodyne_pointcloud/calibration.h>
+
+namespace velodyne_pointcloud {
+
+  const std::string NUM_LASERS = "num_lasers";
+  const std::string PITCH = "pitch";
+  const std::string ROLL = "roll";
+  const std::string LASERS = "lasers";
+  const std::string LASER_ID = "laser_id";
+  const std::string ROT_CORRECTION = "rot_correction";
+  const std::string VERT_CORRECTION = "vert_correction";
+  const std::string DIST_CORRECTION = "dist_correction";
+  const std::string TWO_PT_CORRECTION_AVAILABLE = "two_pt_correction_available";
+  const std::string DIST_CORRECTION_X = "dist_correction_x";
+  const std::string DIST_CORRECTION_Y = "dist_correction_y";
+  const std::string VERT_OFFSET_CORRECTION = "vert_offset_correction";
+  const std::string HORIZ_OFFSET_CORRECTION = "vert_offset_correction";
+  const std::string MAX_INTENSITY = "max_intensity";
+  const std::string MIN_INTENSITY = "min_intensity";
+  const std::string FOCAL_DISTANCE = "focal_distance";
+  const std::string FOCAL_SLOPE = "focal_slope";
+
+  void operator >> (const YAML::Node& node, std::pair<int, LaserCorrection>& correction) {
+    node[LASER_ID] >> correction.first;
+    node[ROT_CORRECTION] >> correction.second.rot_correction;
+    node[VERT_CORRECTION] >> correction.second.vert_correction;
+    node[DIST_CORRECTION] >> correction.second.dist_correction;
+    node[TWO_PT_CORRECTION_AVAILABLE] >> correction.second.two_pt_correction_available;
+    node[DIST_CORRECTION_X] >> correction.second.dist_correction_x;
+    node[DIST_CORRECTION_Y] >> correction.second.dist_correction_y;
+    node[VERT_OFFSET_CORRECTION] >> correction.second.vert_offset_correction;
+    node[HORIZ_OFFSET_CORRECTION] >> correction.second.horiz_offset_correction;
+    node[MAX_INTENSITY] >> correction.second.max_intensity;
+    node[MIN_INTENSITY] >> correction.second.min_intensity;
+    node[FOCAL_DISTANCE] >> correction.second.focal_distance;
+    node[FOCAL_SLOPE] >> correction.second.focal_slope;
+
+    // Calculate cached values
+    correction.second.cos_rot_correction = cosf(correction.second.rot_correction);
+    correction.second.sin_rot_correction = sinf(correction.second.rot_correction);
+    correction.second.cos_vert_correction = cosf(correction.second.vert_correction);
+    correction.second.sin_vert_correction = sinf(correction.second.vert_correction);
+  }
+
+  void operator >> (const YAML::Node& node, Calibration& calibration) {
+    int num_lasers;
+    node[NUM_LASERS] >> num_lasers;
+    node[PITCH] >> calibration.pitch;
+    node[ROLL] >> calibration.roll;
+    const YAML::Node& lasers = node[LASERS];
+    calibration.laser_corrections.clear();
+    for (int i = 0; i < num_lasers; i++) {
+      std::pair<int, LaserCorrection> correction;
+      lasers[i] >> correction;
+      calibration.laser_corrections.insert(correction);     
+    }
+  }
+
+  YAML::Emitter& operator << (YAML::Emitter& out, const std::pair<int, LaserCorrection> correction) {
+    out << YAML::BeginMap;
+    out << YAML::Key << LASER_ID << YAML::Value << correction.first;
+    out << YAML::Key << ROT_CORRECTION << YAML::Value << correction.second.rot_correction;
+    out << YAML::Key << VERT_CORRECTION << YAML::Value << correction.second.vert_correction;
+    out << YAML::Key << DIST_CORRECTION << YAML::Value << correction.second.dist_correction;
+    out << YAML::Key << TWO_PT_CORRECTION_AVAILABLE << YAML::Value << correction.second.two_pt_correction_available;
+    out << YAML::Key << DIST_CORRECTION_X << YAML::Value << correction.second.dist_correction_x;
+    out << YAML::Key << DIST_CORRECTION_Y << YAML::Value << correction.second.dist_correction_y;
+    out << YAML::Key << VERT_OFFSET_CORRECTION << YAML::Value << correction.second.vert_offset_correction;
+    out << YAML::Key << HORIZ_OFFSET_CORRECTION << YAML::Value << correction.second.horiz_offset_correction;
+    out << YAML::Key << MAX_INTENSITY << YAML::Value << correction.second.max_intensity;
+    out << YAML::Key << MIN_INTENSITY << YAML::Value << correction.second.min_intensity;
+    out << YAML::Key << FOCAL_DISTANCE << YAML::Value << correction.second.focal_distance;
+    out << YAML::Key << FOCAL_SLOPE << YAML::Value << correction.second.focal_slope;
+    out << YAML::EndMap;
+    return out;
+  }
+
+  YAML::Emitter& operator << (YAML::Emitter& out, const Calibration& calibration) {
+    out << YAML::BeginMap;
+    out << YAML::Key << NUM_LASERS << YAML::Value << calibration.laser_corrections.size();
+    out << YAML::Key << PITCH << YAML::Value << calibration.pitch;
+    out << YAML::Key << ROLL << YAML::Value << calibration.roll;
+    out << YAML::Key << LASERS << YAML::Value << YAML::BeginSeq;
+    for (std::map<int, LaserCorrection>::const_iterator it = calibration.laser_corrections.begin();
+         it != calibration.laser_corrections.end(); it++) {
+      out << *it; 
+    }
+    out << YAML::EndSeq;
+    out << YAML::EndMap;
+    return out;
+  }
+
+  void Calibration::read(const std::string& calibration_file) {
+    std::ifstream fin(calibration_file.c_str());
+    if (!fin.is_open()) {
+      initialized = false;
+      return;
+    }
+    YAML::Parser parser(fin);
+    YAML::Node doc;
+    parser.GetNextDocument(doc);
+    doc >> *this;
+    fin.close();
+    initialized = true;
+  }
+
+  void Calibration::write(const std::string& calibration_file) {
+    std::ofstream fout(calibration_file.c_str());
+    YAML::Emitter out;
+    out << *this;
+    fout << out.c_str();
+    fout.close();
+  }
+
+} /* velodyne_pointcloud */