[velodyne_pointcloud] add_two_pt scripts (#473)

* [velodyne_pointcloud] add_two_pt scripts

see issues: #109 #295
and commit: f30d687

* [velodyne_pointcloud] gen_calibration: add two_pt

see issues: #109 #295
and commit: f30d687

* [velodyne_pointcloud] add two_pt in params/64e_*

* [velodyne_pointcloud] update test_calibration

Author: PierrickKoch

velodyne_pointcloud/params/64e_s2.1-sztaki.yaml

@@ -0,0 +1,18 @@
+#!/usr/bin/python
+"""
+usage: add_two_pt.py < calibration.yaml > calibration_two_pt.yaml
+
+In order to take into acount *HDL-64* correction_{x,y}, related to 2012 merge:
+https://github.com/ros-drivers/velodyne/commit/f30d68735c47312aa73d29203ddb16abc01357f4
+
+https://github.com/ros-drivers/velodyne/blob/master/velodyne_pointcloud/src/lib/rawdata.cc#L438
+https://github.com/ros-drivers/velodyne/blob/master/velodyne_pointcloud/src/lib/calibration.cc#L70
+"""
+import sys
+import yaml
+
+calibration = yaml.safe_load(sys.stdin)
+for laser in calibration['lasers']:
+    laser['two_pt_correction_available'] = True
+
+print(yaml.safe_dump(calibration))

velodyne_pointcloud/params/64e_s3-xiesc.yaml

@@ -199,6 +199,14 @@ def addLaserCalibration(laser_num, key, val):
 # TODO: make sure all required fields are present.
 # (Which ones are required?)
 
+
+# fix #473 : take into acount HDL-64 correction_{x,y}, related to:
+#            commit/f30d68735c47312aa73d29203ddb16abc01357f4
+for laser in calibration['lasers']:
+    if laser.get('dist_correction_x', 0) and laser.get('dist_correction_y', 0):
+        laser['two_pt_correction_available'] = True
+
+
 if calibrationGood:
 
     # write calibration data to YAML file

velodyne_pointcloud/params/64e_utexas.yaml

@@ -110,15 +110,15 @@ TEST(Calibration, hdl64e)
 
   // check some values for the first laser:
   LaserCorrection laser = calibration.laser_corrections[0];
-  EXPECT_FALSE(laser.two_pt_correction_available);
+  EXPECT_TRUE(laser.two_pt_correction_available);
   EXPECT_FLOAT_EQ(laser.vert_correction, -0.124932751059532);
   EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
   EXPECT_EQ(laser.max_intensity, 255);
   EXPECT_EQ(laser.min_intensity, 0);
 
   // check similar values for the last laser:
   laser = calibration.laser_corrections[63];
-  EXPECT_FALSE(laser.two_pt_correction_available);
+  EXPECT_TRUE(laser.two_pt_correction_available);
   EXPECT_FLOAT_EQ(laser.vert_correction, -0.209881335496902);
   EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.0);
   EXPECT_EQ(laser.max_intensity, 255);
@@ -134,15 +134,15 @@ TEST(Calibration, hdl64e_s21)
 
   // check some values for the first laser:
   LaserCorrection laser = calibration.laser_corrections[0];
-  EXPECT_FALSE(laser.two_pt_correction_available);
+  EXPECT_TRUE(laser.two_pt_correction_available);
   EXPECT_FLOAT_EQ(laser.vert_correction, -0.15304134919741974);
   EXPECT_FLOAT_EQ(laser.horiz_offset_correction, 0.025999999);
   EXPECT_EQ(laser.max_intensity, 235);
   EXPECT_EQ(laser.min_intensity, 30);
 
   // check similar values for the last laser:
   laser = calibration.laser_corrections[63];
-  EXPECT_FALSE(laser.two_pt_correction_available);
+  EXPECT_TRUE(laser.two_pt_correction_available);
   EXPECT_FLOAT_EQ(laser.vert_correction, -0.2106649408137298);
   EXPECT_FLOAT_EQ(laser.horiz_offset_correction, -0.025999999);
   EXPECT_EQ(laser.max_intensity, 255);