Added Lidar Python interface (#973)

Signed-off-by: ahcorde <[email protected]>

python/CMakeLists.txt

@@ -54,6 +54,7 @@ pybind11_add_module(sdformat SHARED
   src/sdf/pyIMU.cc
   src/sdf/pyJoint.cc
   src/sdf/pyJointAxis.cc
+  src/sdf/pyLidar.cc
   src/sdf/pyLight.cc
   src/sdf/pyLink.cc
   src/sdf/pyMagnetometer.cc
@@ -106,6 +107,7 @@ if (BUILD_TESTING)
     pyIMU_TEST
     pyJoint_TEST
     pyJointAxis_TEST
+    pyLidar_TEST
     pyLight_TEST
     pyLink_TEST
     pyMagnetometer_TEST

python/src/sdf/_ignition_sdformat_pybind11.cc

@@ -32,6 +32,7 @@
 #include "pyIMU.hh"
 #include "pyJoint.hh"
 #include "pyJointAxis.hh"
+#include "pyLidar.hh"
 #include "pyLight.hh"
 #include "pyLink.hh"
 #include "pyMagnetometer.hh"
@@ -70,6 +71,7 @@ PYBIND11_MODULE(sdformat, m) {
   sdf::python::defineIMU(m);
   sdf::python::defineJoint(m);
   sdf::python::defineJointAxis(m);
+  sdf::python::defineLidar(m);
   sdf::python::defineLight(m);
   sdf::python::defineLink(m);
   sdf::python::defineMagnetometer(m);

python/src/sdf/pyLidar.cc

@@ -0,0 +1,114 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#include "pyLidar.hh"
+
+#include <pybind11/operators.h>
+#include <pybind11/pybind11.h>
+
+#include "sdf/Lidar.hh"
+
+using namespace pybind11::literals;
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/////////////////////////////////////////////////
+void defineLidar(pybind11::object module)
+{
+  pybind11::class_<sdf::Lidar> geometryModule(module, "Lidar");
+  geometryModule
+    .def(pybind11::init<>())
+    .def(pybind11::init<sdf::Lidar>())
+    .def(pybind11::self == pybind11::self)
+    .def(pybind11::self != pybind11::self)
+    .def("horizontal_scan_samples", &sdf::Lidar::HorizontalScanSamples,
+         "Get the number of lidar rays horizontally to generate per laser "
+         "sweep.")
+    .def("set_horizontal_scan_samples", &sdf::Lidar::SetHorizontalScanSamples,
+         "Get the number of lidar rays horizontally to generate per laser "
+         "sweep.")
+    .def("horizontal_scan_resolution", &sdf::Lidar::HorizontalScanResolution,
+         "Get the resolution for horizontal scan.")
+    .def("set_horizontal_scan_resolution", &sdf::Lidar::SetHorizontalScanResolution,
+         "Set the resolution for horizontal scan.")
+    .def("horizontal_scan_min_angle", &sdf::Lidar::HorizontalScanMinAngle,
+         "Get the minimum angle for horizontal scan.")
+    .def("set_horizontal_scan_min_angle", &sdf::Lidar::SetHorizontalScanMinAngle,
+         "Set the minimum angle for horizontal scan.")
+    .def("horizontal_scan_max_angle", &sdf::Lidar::HorizontalScanMaxAngle,
+         "Get the maximum angle for horizontal scan.")
+    .def("set_horizontal_scan_max_angle", &sdf::Lidar::SetHorizontalScanMaxAngle,
+         "Set the maximum angle for horizontal scan.")
+    .def("vertical_scan_samples", &sdf::Lidar::VerticalScanSamples,
+         "Get the number of lidar rays vertically to generate per laser "
+         "sweep.")
+    .def("set_vertical_scan_samples", &sdf::Lidar::SetVerticalScanSamples,
+         "Set the number of lidar rays vertically to generate per laser "
+         "sweep.")
+    .def("vertical_scan_resolution", &sdf::Lidar::VerticalScanResolution,
+         "Get the resolution for vertical scan.")
+    .def("set_vertical_scan_resolution", &sdf::Lidar::SetVerticalScanResolution,
+         "Set the resolution for vertical scan.")
+    .def("vertical_scan_min_angle", &sdf::Lidar::VerticalScanMinAngle,
+         "Get the minimum angle for vertical scan.")
+    .def("set_vertical_scan_min_angle", &sdf::Lidar::SetVerticalScanMinAngle,
+         "Set the minimum angle for vertical scan.")
+    .def("vertical_scan_max_angle", &sdf::Lidar::VerticalScanMaxAngle,
+         "Get the maximum angle for vertical scan.")
+    .def("set_vertical_scan_max_angle",
+         &sdf::Lidar::SetVerticalScanMaxAngle,
+         "Set the maximum angle for vertical scan.")
+    .def("range_min",
+         &sdf::Lidar::RangeMin,
+         "Get minimum distance for each lidar ray.")
+    .def("set_range_min", &sdf::Lidar::SetRangeMin,
+         "Set minimum distance for each lidar ray.")
+    .def("range_max",
+         &sdf::Lidar::RangeMax,
+         "Get the maximum angle for vertical scan.")
+    .def("set_range_max",
+         &sdf::Lidar::SetRangeMax,
+         "Set the maximum angle for vertical scan.")
+    .def("range_resolution",
+         &sdf::Lidar::RangeResolution,
+         "Get linear resolution of each lidar ray.")
+    .def("set_range_resolution",
+         &sdf::Lidar::SetRangeResolution,
+         "Set linear resolution of each lidar ray.")
+    .def("lidar_noise",
+         &sdf::Lidar::LidarNoise,
+         "Get the noise values for the lidar sensor.")
+    .def("set_lidar_noise", &sdf::Lidar::SetLidarNoise,
+         "Set the noise values for the lidar sensor.")
+    .def("visibility_mask",
+         &sdf::Lidar::VisibilityMask,
+         "Get the visibility mask of a lidar")
+    .def("set_visibility_mask", &sdf::Lidar::SetVisibilityMask,
+         "Set the visibility mask of a lidar.")
+    .def("__copy__", [](const sdf::Lidar &self) {
+      return sdf::Lidar(self);
+    })
+    .def("__deepcopy__", [](const sdf::Lidar &self, pybind11::dict) {
+      return sdf::Lidar(self);
+    }, "memo"_a);
+}
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf

python/src/sdf/pyLidar.hh

@@ -0,0 +1,41 @@
+/*
+ * Copyright (C) 2022 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ */
+
+#ifndef SDFORMAT_PYTHON_LIDAR_HH_
+#define SDFORMAT_PYTHON_LIDAR_HH_
+
+#include <pybind11/pybind11.h>
+
+#include "sdf/Lidar.hh"
+
+#include "sdf/config.hh"
+
+namespace sdf
+{
+// Inline bracket to help doxygen filtering.
+inline namespace SDF_VERSION_NAMESPACE {
+namespace python
+{
+/// Define a pybind11 wrapper for an sdf::Lidar
+/**
+ * \param[in] module a pybind11 module to add the definition to
+ */
+void defineLidar(pybind11::object module);
+}  // namespace python
+}  // namespace SDF_VERSION_NAMESPACE
+}  // namespace sdf
+
+#endif  // SDFORMAT_PYTHON_LIDAR_HH_

python/test/pyLidar_TEST.py

@@ -0,0 +1,85 @@
+# Copyright (C) 2022 Open Source Robotics Foundation
+
+# Licensed under the Apache License, Version 2.0 (the "License")
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+
+#       http://www.apache.org/licenses/LICENSE-2.0
+
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+import copy
+from ignition.math import Angle, Pose3d, Vector3d, Helpers
+from sdformat import Lidar, Error, Noise
+import math
+import unittest
+
+
+class LidarTEST(unittest.TestCase):
+
+  def test_default_construction(self):
+    lidar = Lidar()
+    defaultNoise = Noise()
+    self.assertEqual(defaultNoise, lidar.lidar_noise())
+
+  def test_set(self):
+
+    lidar = Lidar()
+
+    lidar.set_horizontal_scan_samples(123)
+    self.assertEqual(lidar.horizontal_scan_samples(), 123)
+    lidar.set_horizontal_scan_resolution(0.45)
+    self.assertAlmostEqual(lidar.horizontal_scan_resolution(), 0.45)
+    lidar.set_horizontal_scan_min_angle(Angle(0.67))
+    self.assertAlmostEqual(lidar.horizontal_scan_min_angle().radian(), 0.67)
+    lidar.set_horizontal_scan_max_angle(Angle(0.89))
+    self.assertAlmostEqual(lidar.horizontal_scan_max_angle().radian(), 0.89)
+
+    lidar.set_vertical_scan_samples(98)
+    self.assertEqual(lidar.vertical_scan_samples(), 98)
+    lidar.set_vertical_scan_resolution(0.76)
+    self.assertAlmostEqual(lidar.vertical_scan_resolution(), 0.76)
+    lidar.set_vertical_scan_min_angle(Angle(0.54))
+    self.assertAlmostEqual(lidar.vertical_scan_min_angle().radian(), 0.54)
+    lidar.set_vertical_scan_max_angle(Angle(0.321))
+    self.assertAlmostEqual(lidar.vertical_scan_max_angle().radian(), 0.321)
+
+    lidar.set_range_min(1.2)
+    self.assertAlmostEqual(lidar.range_min(), 1.2)
+    lidar.set_range_max(3.4)
+    self.assertAlmostEqual(lidar.range_max(), 3.4)
+    lidar.set_range_resolution(5.6)
+    self.assertAlmostEqual(lidar.range_resolution(), 5.6)
+
+    noise = Noise()
+    noise.set_mean(6.5)
+    noise.set_std_dev(3.79)
+    lidar.set_lidar_noise(noise)
+    self.assertEqual(noise, lidar.lidar_noise())
+
+    lidar.set_horizontal_scan_samples(111)
+    lidar.set_horizontal_scan_resolution(2.2)
+
+    self.assertEqual(Helpers.MAX_UI32, lidar.visibility_mask());
+    lidar.set_visibility_mask(123);
+    self.assertEqual(123, lidar.visibility_mask());
+
+    # Inequality operator
+    lidar2 = Lidar()
+    self.assertNotEqual(lidar2, lidar)
+
+    # Copy constructor
+    lidarCopied = Lidar(lidar)
+    self.assertEqual(lidarCopied, lidar)
+
+    # Assignment operator
+    lidarAssigned = lidar
+    self.assertEqual(lidarAssigned, lidar)
+
+
+if __name__ == '__main__':
+    unittest.main()