Feature script add two pt ros2 (ros-drivers#498)

* [velodyne_pointcloud] add_two_pt scripts see issues: ros-drivers#109 ros-drivers#295 and commit: f30d687 * [velodyne_pointcloud] gen_calibration: add two_pt see issues: ros-drivers#109 ros-drivers#295 and commit: f30d687 * [velodyne_pointcloud] add two_pt in params/64e_* * [velodyne_pointcloud] update test_calibration * Applying linter fixes for ROS2 port * Linter fix. --------- Co-authored-by: Joshua Whitley <[email protected]>
nobleo · Jul 20, 2023 · 04a7ca7 · 04a7ca7
1 parent d2304ce
commit 04a7ca7
Show file tree

Hide file tree

Showing 6 changed files with 2,306 additions and 730 deletions.
diff --git a/velodyne_pointcloud/params/64e_s2.1-sztaki.yaml b/velodyne_pointcloud/params/64e_s2.1-sztaki.yaml
diff --git a/velodyne_pointcloud/params/64e_s3-xiesc.yaml b/velodyne_pointcloud/params/64e_s3-xiesc.yaml
diff --git a/velodyne_pointcloud/params/64e_utexas.yaml b/velodyne_pointcloud/params/64e_utexas.yaml
diff --git a/velodyne_pointcloud/scripts/add_two_pt.py b/velodyne_pointcloud/scripts/add_two_pt.py
@@ -0,0 +1,52 @@
+#!/usr/bin/python3
+
+# Copyright 2023 Pierrick Koch
+# All rights reserved.
+#
+# Software License Agreement (BSD License 2.0)
+#
+# Redistribution and use in source and binary forms, with or without
+# modification, are permitted provided that the following conditions
+# are met:
+#
+# * Redistributions of source code must retain the above copyright
+#   notice, this list of conditions and the following disclaimer.
+# * Redistributions in binary form must reproduce the above
+#   copyright notice, this list of conditions and the following
+#   disclaimer in the documentation and/or other materials provided
+#   with the distribution.
+# * Neither the name of {copyright_holder} nor the names of its
+#   contributors may be used to endorse or promote products derived
+#   from this software without specific prior written permission.
+#
+# THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS
+# "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT
+# LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS
+# FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE
+# COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT,
+# INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING,
+# BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+# LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
+# CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT
+# LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN
+# ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE
+# POSSIBILITY OF SUCH DAMAGE.
+
+"""
+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))
diff --git a/velodyne_pointcloud/scripts/gen_calibration.py b/velodyne_pointcloud/scripts/gen_calibration.py
@@ -61,6 +61,12 @@ def __init__(self):
             print('converting "' + self.xml_file + '" to "' + self.yaml_file + '"')
             self.parse_xml()
 
+        # fix #473 : take into acount HDL-64 correction_{x,y}, related to:
+        #            commit/f30d68735c47312aa73d29203ddb16abc01357f4
+        for laser in self.calibration['lasers']:
+            if laser.get('dist_correction_x', 0) and laser.get('dist_correction_y', 0):
+                laser['two_pt_correction_available'] = True
+
         if self.calibration_good:
             self.write_calibration_file()
 

diff --git a/velodyne_pointcloud/tests/test_calibration.cpp b/velodyne_pointcloud/tests/test_calibration.cpp
@@ -109,15 +109,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);
@@ -131,15 +131,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);