From 3fc159b475f2660e09011c59d27cb97d744fd253 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 Dec 2021 17:47:30 +0100 Subject: [PATCH 1/5] Added SignalStats pybind11 interface (#343) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/SignalStats.cc | 551 ++++++++++++++++++ src/python_pybind11/src/SignalStats.hh | 92 +++ .../src/_ignition_math_pybind11.cc | 12 + .../test}/SignalStats_TEST.py | 97 +-- 6 files changed, 706 insertions(+), 49 deletions(-) create mode 100644 src/python_pybind11/src/SignalStats.cc create mode 100644 src/python_pybind11/src/SignalStats.hh rename src/{python => python_pybind11/test}/SignalStats_TEST.py (86%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index ed4872a7a..c724e5d04 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -88,7 +88,6 @@ if (PYTHONLIBS_FOUND) # Add the Python tests set(python_tests python_TEST - SignalStats_TEST SphericalCoordinates_TEST Vector3Stats_TEST ) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 73e9232fc..29f23dc02 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -28,6 +28,7 @@ if (${pybind11_FOUND}) src/RollingMean.cc src/RotationSpline.cc src/SemanticVersion.cc + src/SignalStats.cc src/Spline.cc src/StopWatch.cc src/Temperature.cc @@ -111,6 +112,7 @@ if (${pybind11_FOUND}) RollingMean_TEST RotationSpline_TEST SemanticVersion_TEST + SignalStats_TEST Sphere_TEST Spline_TEST StopWatch_TEST diff --git a/src/python_pybind11/src/SignalStats.cc b/src/python_pybind11/src/SignalStats.cc new file mode 100644 index 000000000..69b31e661 --- /dev/null +++ b/src/python_pybind11/src/SignalStats.cc @@ -0,0 +1,551 @@ +/* + * Copyright (C) 2021 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 + +#include +#include + +#include "SignalStats.hh" +#include + +namespace ignition +{ +namespace math +{ +namespace python +{ +////////////////////////////////////////////////// +void defineMathSignalStats(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalStats; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def("count", &Class::Count, "Get number of data points in first statistic.") + .def("reset", &Class::Reset, "Forget all previous data.") + .def("map", &Class::Map, + "Get the current values of each statistical measure, " + "stored in a map using the short name as the key.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measures.") + .def("insert_statistic", + &Class::InsertStatistic, + "Add a new type of statistic.") + .def("insert_statistics", + &Class::InsertStatistics, + "Add multiple statistics."); +} + +////////////////////////////////////////////////// +class SignalStatisticTrampoline : public ignition::math::SignalStatistic +{ +public: + SignalStatisticTrampoline() : SignalStatistic() {} + explicit SignalStatisticTrampoline(const SignalStatistic &_ss) : + SignalStatistic(_ss) {} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + size_t Count() const override + { + PYBIND11_OVERLOAD_PURE( + size_t, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Count, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } + // Trampoline (need one for each virtual function) + void Reset() override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Reset, + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalStatistic(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalStatistic; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def(py::init()) + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure.") + .def("reset", + &Class::Reset, + "Forget all previous data."); +} + +////////////////////////////////////////////////// +class SignalVarianceTrampoline : public ignition::math::SignalVariance { +public: + // Inherit the constructors + SignalVarianceTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalVariance(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalVariance; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMaximumTrampoline : public ignition::math::SignalMaximum +{ +public: + // Inherit the constructors + SignalMaximumTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMaximum(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMaximum; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMinimumTrampoline : public ignition::math::SignalMinimum +{ +public: + // Inherit the constructors + SignalMinimumTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMinimum(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMinimum; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMeanTrampoline : public ignition::math::SignalMean +{ +public: + // Inherit the constructors + SignalMeanTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMean(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMean; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalRootMeanSquareTrampoline : + public ignition::math::SignalRootMeanSquare +{ +public: + // Inherit the constructors + SignalRootMeanSquareTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalRootMeanSquare; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} + +////////////////////////////////////////////////// +class SignalMaxAbsoluteValueTrampoline : + public ignition::math::SignalMaxAbsoluteValue +{ +public: + // Inherit the constructors + SignalMaxAbsoluteValueTrampoline(){} + + // Trampoline (need one for each virtual function) + double Value() const override + { + PYBIND11_OVERLOAD_PURE( + double, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + Value, + ); + } + // Trampoline (need one for each virtual function) + std::string ShortName() const override + { + PYBIND11_OVERLOAD_PURE( + std::string, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + ShortName, + ); + } + // Trampoline (need one for each virtual function) + void InsertData(const double _data) override + { + PYBIND11_OVERLOAD_PURE( + void, // Return type (ret_type) + ignition::math::SignalStatistic, // Parent class (cname) + // Name of function in C++ (must match Python name) (fn) + InsertData, + _data // Argument(s) (...) + ); + } +}; + +////////////////////////////////////////////////// +void defineMathSignalMaxAbsoluteValue(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SignalMaxAbsoluteValue; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("count", + &Class::Count, + "Get number of data points in measurement.") + .def("value", + &Class::Value, + "Get a short version of the name of this statistical measure.") + .def("short_name", + &Class::ShortName, + "Get a short version of the name of this statistical measure.") + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measure."); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/SignalStats.hh b/src/python_pybind11/src/SignalStats.hh new file mode 100644 index 000000000..7e92e1940 --- /dev/null +++ b/src/python_pybind11/src/SignalStats.hh @@ -0,0 +1,92 @@ +/* + * Copyright (C) 2021 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 IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ +#define IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ + +#include +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::SignalStats +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalStats(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalStatistic +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalStatistic(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMaximum +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMaximum(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMinimum +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMinimum(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalVariance +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalVariance(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMaxAbsoluteValue +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMaxAbsoluteValue( + py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalRootMeanSquare +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalRootMeanSquare(py::module &m, const std::string &typestr); + +/// Define a pybind11 wrapper for an ignition::math::SignalMean +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSignalMean(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__SIGNALSTATS_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 1e1aa8b4b..61d4dd2a6 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -42,6 +42,7 @@ #include "RollingMean.hh" #include "RotationSpline.hh" #include "SemanticVersion.hh" +#include "SignalStats.hh" #include "Sphere.hh" #include "Spline.hh" #include "StopWatch.hh" @@ -89,6 +90,17 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathRollingMean(m, "RollingMean"); + ignition::math::python::defineMathSignalStats(m, "SignalStats"); + ignition::math::python::defineMathSignalStatistic(m, "SignalStatistic"); + ignition::math::python::defineMathSignalVariance(m, "SignalVariance"); + ignition::math::python::defineMathSignalMaximum(m, "SignalMaximum"); + ignition::math::python::defineMathSignalMinimum(m, "SignalMinimum"); + ignition::math::python::defineMathSignalMaxAbsoluteValue( + m, "SignalMaxAbsoluteValue"); + ignition::math::python::defineMathSignalRootMeanSquare( + m, "SignalRootMeanSquare"); + ignition::math::python::defineMathSignalMean(m, "SignalMean"); + ignition::math::python::defineMathRotationSpline(m, "RotationSpline"); ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); diff --git a/src/python/SignalStats_TEST.py b/src/python_pybind11/test/SignalStats_TEST.py similarity index 86% rename from src/python/SignalStats_TEST.py rename to src/python_pybind11/test/SignalStats_TEST.py index e8497e6d2..26e4e4974 100644 --- a/src/python/SignalStats_TEST.py +++ b/src/python_pybind11/test/SignalStats_TEST.py @@ -372,7 +372,7 @@ def test_signal_variance_random_values(self): def test_signal_stats_constructor(self): # Constructor stats = SignalStats() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertEqual(stats.count(), 0) stats2 = SignalStats(stats) @@ -380,110 +380,111 @@ def test_signal_stats_constructor(self): # reset stats.reset() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertEqual(stats.count(), 0) def test_01_signal_stats_intern_statistic(self): # insert static stats = SignalStats() - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("max")) self.assertFalse(stats.insert_statistic("max")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("maxAbs")) self.assertFalse(stats.insert_statistic("maxAbs")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("mean")) self.assertFalse(stats.insert_statistic("mean")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("min")) self.assertFalse(stats.insert_statistic("min")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("rms")) self.assertFalse(stats.insert_statistic("rms")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertTrue(stats.insert_statistic("var")) self.assertFalse(stats.insert_statistic("var")) - self.assertFalse(stats.map().empty()) + self.assertFalse(len(stats.map()) == 0) self.assertFalse(stats.insert_statistic("FakeStatistic")) # map with no data map = stats.map() - self.assertFalse(map.empty()) - self.assertEqual(map.size(), 6) - self.assertEqual(map.count("max"), 1) - self.assertEqual(map.count("maxAbs"), 1) - self.assertEqual(map.count("mean"), 1) - self.assertEqual(map.count("min"), 1) - self.assertEqual(map.count("rms"), 1) - self.assertEqual(map.count("var"), 1) - self.assertEqual(map.count("FakeStatistic"), 0) + self.assertFalse(len(map) == 0) + self.assertEqual(len(map), 6) + + self.assertEqual("max" in map.keys(), 1) + self.assertEqual("maxAbs" in map.keys(), 1) + self.assertEqual("mean" in map.keys(), 1) + self.assertEqual("min" in map.keys(), 1) + self.assertEqual("rms" in map.keys(), 1) + self.assertEqual("var" in map.keys(), 1) + self.assertEqual("FakeStatistic" in map.keys(), 0) stats2 = SignalStats(stats) map2 = stats2.map() - self.assertFalse(map2.empty()) - self.assertEqual(map.size(), map2.size()) - self.assertEqual(map.count("max"), map2.count("max")) - self.assertEqual(map.count("maxAbs"), map2.count("maxAbs")) - self.assertEqual(map.count("mean"), map2.count("mean")) - self.assertEqual(map.count("min"), map2.count("min")) - self.assertEqual(map.count("rms"), map2.count("rms")) - self.assertEqual(map.count("var"), map2.count("var")) - self.assertEqual(map.count("FakeStatistic"), - map2.count("FakeStatistic")) + self.assertFalse(len(map2) == 0) + self.assertEqual(len(map), len(map2)) + self.assertEqual("max" in map.keys(), "max" in map2.keys()) + self.assertEqual("maxAbs" in map.keys(), "maxAbs" in map2.keys()) + self.assertEqual("mean" in map.keys(), "mean" in map2.keys()) + self.assertEqual("min" in map.keys(), "min" in map2.keys()) + self.assertEqual("rms" in map.keys(), "rms" in map2.keys()) + self.assertEqual("var" in map.keys(), "var" in map2.keys()) + self.assertEqual("FakeStatistic" in map.keys(), + "FakeStatistic" in map2.keys()) def test_02_signal_stats_intern_statistic(self): # insert statics stats = SignalStats() self.assertFalse(stats.insert_statistics("")) - self.assertTrue(stats.map().empty()) + self.assertTrue(len(stats.map()) == 0) self.assertTrue(stats.insert_statistics("maxAbs,rms")) - self.assertEqual(stats.map().size(), 2) + self.assertEqual(len(stats.map()), 2) self.assertFalse(stats.insert_statistics("maxAbs,rms")) self.assertFalse(stats.insert_statistics("maxAbs")) self.assertFalse(stats.insert_statistics("rms")) - self.assertEqual(stats.map().size(), 2) + self.assertEqual(len(stats.map()), 2) self.assertFalse(stats.insert_statistics("mean,FakeStatistic")) - self.assertEqual(stats.map().size(), 3) + self.assertEqual(len(stats.map()), 3) self.assertFalse(stats.insert_statistics("var,FakeStatistic")) - self.assertEqual(stats.map().size(), 4) + self.assertEqual(len(stats.map()), 4) self.assertFalse(stats.insert_statistics("max,FakeStatistic")) - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) self.assertFalse(stats.insert_statistics("min,FakeStatistic")) - self.assertEqual(stats.map().size(), 6) + self.assertEqual(len(stats.map()), 6) self.assertFalse(stats.insert_statistics("FakeStatistic")) - self.assertEqual(stats.map().size(), 6) + self.assertEqual(len(stats.map()), 6) # map with no data map = stats.map() - self.assertFalse(map.empty()) - self.assertEqual(map.size(), 6) - self.assertEqual(map.count("max"), 1) - self.assertEqual(map.count("maxAbs"), 1) - self.assertEqual(map.count("mean"), 1) - self.assertEqual(map.count("min"), 1) - self.assertEqual(map.count("rms"), 1) - self.assertEqual(map.count("var"), 1) - self.assertEqual(map.count("FakeStatistic"), 0) + self.assertFalse(len(map) == 0) + self.assertEqual(len(map), 6) + self.assertEqual("max" in map.keys(), 1) + self.assertEqual("maxAbs" in map.keys(), 1) + self.assertEqual("mean" in map.keys(), 1) + self.assertEqual("min" in map.keys(), 1) + self.assertEqual("rms" in map.keys(), 1) + self.assertEqual("var" in map.keys(), 1) + self.assertEqual("FakeStatistic" in map.keys(), 0) def test_signal_stats_alternating_values(self): # Add some statistics stats = SignalStats() self.assertTrue(stats.insert_statistics("max,maxAbs,mean,min,rms")) - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) # No data yet self.assertEqual(stats.count(), 0) @@ -505,7 +506,7 @@ def test_signal_stats_alternating_values(self): copy = SignalStats(stats) self.assertEqual(copy.count(), 2) map = stats.map() - self.assertEqual(map.size(), 5) + self.assertEqual(len(map), 5) self.assertAlmostEqual(map["max"], value) self.assertAlmostEqual(map["maxAbs"], value) self.assertAlmostEqual(map["min"], -value) @@ -513,7 +514,7 @@ def test_signal_stats_alternating_values(self): self.assertAlmostEqual(map["mean"], 0.0) stats.reset() - self.assertEqual(stats.map().size(), 5) + self.assertEqual(len(stats.map()), 5) self.assertEqual(stats.count(), 0) map = stats.map() self.assertAlmostEqual(map["max"], 0.0) From 5e25570504a6702dfbbb77cf20dda0c52df1fbba Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 Dec 2021 17:50:04 +0100 Subject: [PATCH 2/5] Small fixed in doxygen (#355) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- include/ignition/math/PID.hh | 4 ++-- include/ignition/math/Quaternion.hh | 6 +++--- include/ignition/math/Spline.hh | 4 ++-- 3 files changed, 7 insertions(+), 7 deletions(-) diff --git a/include/ignition/math/PID.hh b/include/ignition/math/PID.hh index 59a41ad33..3291b3c5b 100644 --- a/include/ignition/math/PID.hh +++ b/include/ignition/math/PID.hh @@ -100,7 +100,7 @@ namespace ignition /// \param[in] _i integral gain value public: void SetIGain(const double _i); - /// \brief Set the derivtive Gain. + /// \brief Set the derivative Gain. /// \param[in] _d derivative gain value public: void SetDGain(const double _d); @@ -149,7 +149,7 @@ namespace ignition /// \return The maximum value public: double CmdMax() const; - /// \brief Get the maximum value for the command. + /// \brief Get the minimun value for the command. /// \return The maximum value public: double CmdMin() const; diff --git a/include/ignition/math/Quaternion.hh b/include/ignition/math/Quaternion.hh index ba13d6dd2..86cae56f1 100644 --- a/include/ignition/math/Quaternion.hh +++ b/include/ignition/math/Quaternion.hh @@ -449,8 +449,8 @@ namespace ignition } /// \brief Return rotation as axis and angle - /// \param[in] _axis rotation axis - /// \param[in] _angle ccw angle in radians + /// \param[out] _axis rotation axis + /// \param[out] _angle ccw angle in radians public: void ToAxis(Vector3 &_axis, T &_angle) const { T len = this->qx*this->qx + this->qy*this->qy + this->qz*this->qz; @@ -590,7 +590,7 @@ namespace ignition } } - /// \brief Scale a Quaternionion + /// \brief Scale a Quaternion /// \param[in] _scale Amount to scale this rotation public: void Scale(T _scale) { diff --git a/include/ignition/math/Spline.hh b/include/ignition/math/Spline.hh index 2fc3703f9..ee2658223 100644 --- a/include/ignition/math/Spline.hh +++ b/include/ignition/math/Spline.hh @@ -58,13 +58,13 @@ namespace ignition /// \return arc length or INF on error. public: double ArcLength() const; - /// \brief Gets spline arc length up to + /// \brief Sets spline arc length up to /// a given parameter value \p _t. /// \param[in] _t parameter value (range 0 to 1). /// \return arc length up to \p _t or INF on error. public: double ArcLength(const double _t) const; - /// \brief Gets a spline segment arc length. + /// \brief Sets a spline segment arc length. /// \param[in] _index of the spline segment. /// \param[in] _t parameter value (range 0 to 1). /// \return arc length of a given segment up to From 5fa6f7858a873448c9fa09be23dafc99f0f8863d Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 Dec 2021 18:02:53 +0100 Subject: [PATCH 3/5] kmeans example in cpp and python (#356) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández --- examples/CMakeLists.txt | 3 ++ examples/kmeans.cc | 64 +++++++++++++++++++++++++++++++++++++++++ examples/kmeans.py | 51 ++++++++++++++++++++++++++++++++ 3 files changed, 118 insertions(+) create mode 100644 examples/kmeans.cc create mode 100644 examples/kmeans.py diff --git a/examples/CMakeLists.txt b/examples/CMakeLists.txt index 76e0be122..043fd7299 100644 --- a/examples/CMakeLists.txt +++ b/examples/CMakeLists.txt @@ -31,5 +31,8 @@ target_link_libraries(rand_example ignition-math${IGN_MATH_VER}::ignition-math${ add_executable(graph_example graph_example.cc) target_link_libraries(graph_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) +add_executable(kmeans kmeans.cc) +target_link_libraries(kmeans ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) + add_executable(color_example color_example.cc) target_link_libraries(color_example ignition-math${IGN_MATH_VER}::ignition-math${IGN_MATH_VER}) diff --git a/examples/kmeans.cc b/examples/kmeans.cc new file mode 100644 index 000000000..6f02d2091 --- /dev/null +++ b/examples/kmeans.cc @@ -0,0 +1,64 @@ +/* + * Copyright (C) 2021 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 +#include + +#include + +int main(int argc, char **argv) +{ + // Create some observations. + std::vector obs; + obs.push_back(ignition::math::Vector3d(1.0, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(1.1, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(1.2, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(1.3, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(1.4, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(5.0, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(5.1, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(5.2, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(5.3, 1.0, 0.0)); + obs.push_back(ignition::math::Vector3d(5.4, 1.0, 0.0)); + + // Initialize Kmeans with two partitions. + ignition::math::Kmeans kmeans(obs); + + std::vector obsCopy; + obsCopy = kmeans.Observations(); + + for (auto &elem : obsCopy) + elem += ignition::math::Vector3d(0.1, 0.2, 0.0); + + // append more observations + kmeans.AppendObservations(obsCopy); + + // cluster + std::vector centroids; + std::vector labels; + auto result = kmeans.Cluster(2, centroids, labels); + + // Check that there are two centroids. + std::cout << "Is there a solution ? " << result << std::endl; + std::cout << "There are " << centroids.size() << " centroids" << std::endl; + std::cout << "labels: ["; + for (auto &label: labels) + { + std::cout << label << ", "; + } + std::cout << "]" << '\n'; +} diff --git a/examples/kmeans.py b/examples/kmeans.py new file mode 100644 index 000000000..a4507d94f --- /dev/null +++ b/examples/kmeans.py @@ -0,0 +1,51 @@ +# Copyright (C) 2021 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. + + +from ignition.math import Kmeans, Vector3d + +# Create some observations. +obs = list([]) +obs.append(Vector3d(1.0, 1.0, 0.0)) +obs.append(Vector3d(1.1, 1.0, 0.0)) +obs.append(Vector3d(1.2, 1.0, 0.0)) +obs.append(Vector3d(1.3, 1.0, 0.0)) +obs.append(Vector3d(1.4, 1.0, 0.0)) +obs.append(Vector3d(5.0, 1.0, 0.0)) +obs.append(Vector3d(5.1, 1.0, 0.0)) +obs.append(Vector3d(5.2, 1.0, 0.0)) +obs.append(Vector3d(5.3, 1.0, 0.0)) +obs.append(Vector3d(5.4, 1.0, 0.0)) + +# Initialize Kmeans with two partitions. +kmeans = Kmeans(obs) + +# copy original Vector to add more data +obs_copy = list(kmeans.observations()).copy() +obs2 = list([]) + +for idx, a in enumerate(obs_copy): + obs_copy[idx] = a + Vector3d(0.1, 0.2, 0.0) + obs2.append(obs_copy[idx]) + +# Append more observations +kmeans.append_observations(obs2) + +# Calling cluster +result, centroids, labels = kmeans.cluster(2) + +# Check that there are two centroids. +print("Is there a solution ? {}".format(result)); +print("There are {} centroids".format(len(centroids))) +print("labels:", labels) From 4a9374f481da23f18df50fc04d1745c987befa44 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 Dec 2021 18:25:02 +0100 Subject: [PATCH 4/5] Added Vector3Stats pybind11 interface (#351) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit Signed-off-by: Alejandro Hernández Co-authored-by: Louise Poubel --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 2 + src/python_pybind11/src/Vector3Stats.cc | 78 +++++++++++++++++++ src/python_pybind11/src/Vector3Stats.hh | 43 ++++++++++ .../src/_ignition_math_pybind11.cc | 3 + .../test}/Vector3Stats_TEST.py | 57 +++++++------- 6 files changed, 155 insertions(+), 29 deletions(-) create mode 100644 src/python_pybind11/src/Vector3Stats.cc create mode 100644 src/python_pybind11/src/Vector3Stats.hh rename src/{python => python_pybind11/test}/Vector3Stats_TEST.py (73%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index c724e5d04..5b1b4e158 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -89,7 +89,6 @@ if (PYTHONLIBS_FOUND) set(python_tests python_TEST SphericalCoordinates_TEST - Vector3Stats_TEST ) foreach (test ${python_tests}) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index 29f23dc02..d731179f2 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -32,6 +32,7 @@ if (${pybind11_FOUND}) src/Spline.cc src/StopWatch.cc src/Temperature.cc + src/Vector3Stats.cc ) target_link_libraries(math PRIVATE @@ -121,6 +122,7 @@ if (${pybind11_FOUND}) Triangle_TEST Vector2_TEST Vector3_TEST + Vector3Stats_TEST Vector4_TEST ) diff --git a/src/python_pybind11/src/Vector3Stats.cc b/src/python_pybind11/src/Vector3Stats.cc new file mode 100644 index 000000000..047f70508 --- /dev/null +++ b/src/python_pybind11/src/Vector3Stats.cc @@ -0,0 +1,78 @@ +/* + * Copyright (C) 2021 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 + +#include + +#include "Vector3Stats.hh" + +using namespace pybind11::literals; + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathVector3Stats(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::Vector3Stats; + std::string pyclass_name = typestr; + py::class_(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()) + .def(py::init<>()) + .def("insert_data", + &Class::InsertData, + "Add a new sample to the statistical measures") + .def("insert_statistic", + &Class::InsertStatistic, + "Add a new type of statistic.") + .def("insert_statistics", + &Class::InsertStatistics, + "Set the size of the box.") + .def("reset", + &Class::Reset, + "Forget all previous data.") + .def("x", + py::overload_cast<>(&Class::X), + py::return_value_policy::reference, + "Get statistics for x component of signal.") + .def("y", + py::overload_cast<>(&Class::Y), + py::return_value_policy::reference, + "Get statistics for y component of signal.") + .def("z", + py::overload_cast<>(&Class::Z), + py::return_value_policy::reference, + "Get statistics for z component of signal.") + .def("mag", + py::overload_cast<>(&Class::Mag), + py::return_value_policy::reference, + "Get statistics for mag component of signal.") + .def("__copy__", [](const Class &self) { + return Class(self); + }) + .def("__deepcopy__", [](const Class &self, py::dict) { + return Class(self); + }, "memo"_a); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/Vector3Stats.hh b/src/python_pybind11/src/Vector3Stats.hh new file mode 100644 index 000000000..bd1848ac3 --- /dev/null +++ b/src/python_pybind11/src/Vector3Stats.hh @@ -0,0 +1,43 @@ +/* + * Copyright (C) 2021 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 IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ +#define IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ + +#include + +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::Vector3Stats +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathVector3Stats(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__VECTOR3STATS_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index 61d4dd2a6..f07d9f9e6 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -51,6 +51,7 @@ #include "Triangle3.hh" #include "Vector2.hh" #include "Vector3.hh" +#include "Vector3Stats.hh" #include "Vector4.hh" namespace py = pybind11; @@ -103,6 +104,8 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathRotationSpline(m, "RotationSpline"); + ignition::math::python::defineMathVector3Stats(m, "Vector3Stats"); + ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); ignition::math::python::defineMathSpline(m, "Spline"); diff --git a/src/python/Vector3Stats_TEST.py b/src/python_pybind11/test/Vector3Stats_TEST.py similarity index 73% rename from src/python/Vector3Stats_TEST.py rename to src/python_pybind11/test/Vector3Stats_TEST.py index 293c8bda6..d9e04f9d8 100644 --- a/src/python/Vector3Stats_TEST.py +++ b/src/python_pybind11/test/Vector3Stats_TEST.py @@ -36,10 +36,10 @@ def mag(self, _name): def test_vector3stats_constructor(self): # Constructor v3stats = Vector3Stats() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertAlmostEqual(v3stats.x().count(), 0) self.assertAlmostEqual(v3stats.y().count(), 0) self.assertAlmostEqual(v3stats.z().count(), 0) @@ -47,10 +47,10 @@ def test_vector3stats_constructor(self): # Reset v3stats.reset() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertAlmostEqual(v3stats.x().count(), 0) self.assertAlmostEqual(v3stats.y().count(), 0) self.assertAlmostEqual(v3stats.z().count(), 0) @@ -58,35 +58,36 @@ def test_vector3stats_constructor(self): # InsertStatistics v3stats = Vector3Stats() - self.assertTrue(v3stats.x().map().empty()) - self.assertTrue(v3stats.y().map().empty()) - self.assertTrue(v3stats.z().map().empty()) - self.assertTrue(v3stats.mag().map().empty()) + self.assertTrue(len(v3stats.x().map()) == 0) + self.assertTrue(len(v3stats.y().map()) == 0) + self.assertTrue(len(v3stats.z().map()) == 0) + self.assertTrue(len(v3stats.mag().map()) == 0) self.assertTrue(v3stats.insert_statistics("maxAbs")) self.assertFalse(v3stats.insert_statistics("maxAbs")) self.assertFalse(v3stats.insert_statistic("maxAbs")) - self.assertFalse(v3stats.x().map().empty()) - self.assertFalse(v3stats.y().map().empty()) - self.assertFalse(v3stats.z().map().empty()) - self.assertFalse(v3stats.mag().map().empty()) + self.assertFalse(len(v3stats.x().map()) == 0) + self.assertFalse(len(v3stats.y().map()) == 0) + self.assertFalse(len(v3stats.z().map()) == 0) + self.assertFalse(len(v3stats.mag().map()) == 0) # Map with no data map = v3stats.x().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.y().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.z().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) map = v3stats.mag().map() - self.assertAlmostEqual(map.size(), 1) - self.assertAlmostEqual(map.count("maxAbs"), 1) + self.assertAlmostEqual(len(map), 1) + self.assertTrue("maxAbs" in map, 1) # Insert some data self.assertAlmostEqual(v3stats.x().count(), 0) @@ -110,10 +111,10 @@ def test_vector3stats_constructor(self): def test_vector3stats_const_accessor(self): # Const accessors - self.assertTrue(self.stats.x().map().empty()) - self.assertTrue(self.stats.y().map().empty()) - self.assertTrue(self.stats.z().map().empty()) - self.assertTrue(self.stats.mag().map().empty()) + self.assertTrue(len(self.stats.x().map()) == 0) + self.assertTrue(len(self.stats.y().map()) == 0) + self.assertTrue(len(self.stats.z().map()) == 0) + self.assertTrue(len(self.stats.mag().map()) == 0) name = "maxAbs" self.assertTrue(self.stats.insert_statistics(name)) From 2926fb9f232878af170369791ecf3808deff0cb8 Mon Sep 17 00:00:00 2001 From: =?UTF-8?q?Alejandro=20Hern=C3=A1ndez=20Cordero?= Date: Thu, 30 Dec 2021 21:59:21 +0100 Subject: [PATCH 5/5] Added SphericalCoordinates pybind11 interface (#357) MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * Added SphericalCoordinates pybind11 interface Signed-off-by: Alejandro Hernández --- src/python/CMakeLists.txt | 1 - src/python_pybind11/CMakeLists.txt | 2 + .../src/SphericalCoordinates.cc | 132 ++++++++++++++++++ .../src/SphericalCoordinates.hh | 42 ++++++ .../src/_ignition_math_pybind11.cc | 4 + .../test}/SphericalCoordinates_TEST.py | 34 +++-- 6 files changed, 201 insertions(+), 14 deletions(-) create mode 100644 src/python_pybind11/src/SphericalCoordinates.cc create mode 100644 src/python_pybind11/src/SphericalCoordinates.hh rename src/{python => python_pybind11/test}/SphericalCoordinates_TEST.py (94%) diff --git a/src/python/CMakeLists.txt b/src/python/CMakeLists.txt index 5b1b4e158..2162db6f7 100644 --- a/src/python/CMakeLists.txt +++ b/src/python/CMakeLists.txt @@ -88,7 +88,6 @@ if (PYTHONLIBS_FOUND) # Add the Python tests set(python_tests python_TEST - SphericalCoordinates_TEST ) foreach (test ${python_tests}) diff --git a/src/python_pybind11/CMakeLists.txt b/src/python_pybind11/CMakeLists.txt index d731179f2..7c618b202 100644 --- a/src/python_pybind11/CMakeLists.txt +++ b/src/python_pybind11/CMakeLists.txt @@ -29,6 +29,7 @@ if (${pybind11_FOUND}) src/RotationSpline.cc src/SemanticVersion.cc src/SignalStats.cc + src/SphericalCoordinates.cc src/Spline.cc src/StopWatch.cc src/Temperature.cc @@ -115,6 +116,7 @@ if (${pybind11_FOUND}) SemanticVersion_TEST SignalStats_TEST Sphere_TEST + SphericalCoordinates_TEST Spline_TEST StopWatch_TEST Temperature_TEST diff --git a/src/python_pybind11/src/SphericalCoordinates.cc b/src/python_pybind11/src/SphericalCoordinates.cc new file mode 100644 index 000000000..9bf0dee7c --- /dev/null +++ b/src/python_pybind11/src/SphericalCoordinates.cc @@ -0,0 +1,132 @@ +/* + * Copyright (C) 2021 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 + +#include "SphericalCoordinates.hh" +#include +#include + +namespace ignition +{ +namespace math +{ +namespace python +{ +void defineMathSphericalCoordinates(py::module &m, const std::string &typestr) +{ + using Class = ignition::math::SphericalCoordinates; + std::string pyclass_name = typestr; + + py::class_ sphericalCoordinates(m, + pyclass_name.c_str(), + py::buffer_protocol(), + py::dynamic_attr()); + sphericalCoordinates + .def(py::init<>()) + .def(py::init()) + .def(py::init()) + .def(py::init()) + .def(py::self != py::self) + .def(py::self == py::self) + .def("spherical_from_local_position", + &Class::SphericalFromLocalPosition, + "Convert a Cartesian position vector to geodetic coordinates.") + .def("global_from_local_velocity", + &Class::GlobalFromLocalVelocity, + "Convert a Cartesian velocity vector in the local frame " + " to a global Cartesian frame with components East, North, Up") + .def("convert", + py::overload_cast(&Class::Convert), + "Convert a string to a SurfaceType.") + .def("convert", + py::overload_cast(&Class::Convert), + "Convert a SurfaceType to a string.") + .def("distance", + &Class::Distance, + "Get the distance between two points expressed in geographic " + "latitude and longitude. It assumes that both points are at sea level." + " Example: _latA = 38.0016667 and _lonA = -123.0016667) represents " + "the point with latitude 38d 0'6.00\"N and longitude 123d 0'6.00\"W.") + .def("surface", + &Class::Surface, + "Get SurfaceType currently in use.") + .def("latitude_reference", + &Class::LatitudeReference, + "Get reference geodetic latitude.") + .def("longitude_reference", + &Class::LongitudeReference, + "Get reference longitude.") + .def("elevation_reference", + &Class::ElevationReference, + "Get reference elevation in meters.") + .def("heading_offset", + &Class::HeadingOffset, + "Get heading offset for the reference frame, expressed as " + "angle from East to x-axis, or equivalently " + "from North to y-axis.") + .def("set_surface", + &Class::SetSurface, + "Set SurfaceType for planetary surface model.") + .def("set_latitude_reference", + &Class::SetLatitudeReference, + "Set reference geodetic latitude.") + .def("set_longitude_reference", + &Class::SetLongitudeReference, + "Set reference longitude.") + .def("set_elevation_reference", + &Class::SetElevationReference, + "Set reference elevation above sea level in meters.") + .def("set_heading_offset", + &Class::SetHeadingOffset, + "Set heading angle offset for the frame.") + .def("local_from_spherical_position", + &Class::LocalFromSphericalPosition, + "Convert a geodetic position vector to Cartesian coordinates.") + .def("local_from_global_velocity", + &Class::LocalFromGlobalVelocity, + "Convert a Cartesian velocity vector with components East, " + "North, Up to a local cartesian frame vector XYZ.") + .def("update_transformation_matrix", + &Class::UpdateTransformationMatrix, + "Update coordinate transformation matrix with reference location") + .def("position_transform", + &Class::PositionTransform, + "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " + "Spherical coordinates use radians, while the other frames use " + "meters.") + .def("velocity_transform", + &Class::VelocityTransform, + "Convert between velocity in SPHERICAL/ECEF/LOCAL/GLOBAL frame " + "Spherical coordinates use radians, while the other frames use " + "meters."); + + py::enum_(sphericalCoordinates, "CoordinateType") + .value("SPHERICAL", Class::CoordinateType::SPHERICAL) + .value("ECEF", Class::CoordinateType::ECEF) + .value("GLOBAL", Class::CoordinateType::GLOBAL) + .value("LOCAL", Class::CoordinateType::LOCAL) + .value("LOCAL2", Class::CoordinateType::LOCAL2) + .export_values(); + py::enum_(sphericalCoordinates, "SurfaceType") + .value("EARTH_WGS84", Class::SurfaceType::EARTH_WGS84) + .export_values(); +} +} // namespace python +} // namespace math +} // namespace ignition diff --git a/src/python_pybind11/src/SphericalCoordinates.hh b/src/python_pybind11/src/SphericalCoordinates.hh new file mode 100644 index 000000000..e7a2b83cd --- /dev/null +++ b/src/python_pybind11/src/SphericalCoordinates.hh @@ -0,0 +1,42 @@ +/* + * Copyright (C) 2021 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 IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ +#define IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ + +#include +#include + +namespace py = pybind11; + +namespace ignition +{ +namespace math +{ +namespace python +{ +/// Define a pybind11 wrapper for an ignition::math::SphericalCoordinates +/** + * \param[in] module a pybind11 module to add the definition to + * \param[in] typestr name of the type used by Python + */ +void defineMathSphericalCoordinates(py::module &m, const std::string &typestr); +} // namespace python +} // namespace math +} // namespace ignition + +#endif // IGNITION_MATH_PYTHON__SPHERICALCOORDINATES_HH_ diff --git a/src/python_pybind11/src/_ignition_math_pybind11.cc b/src/python_pybind11/src/_ignition_math_pybind11.cc index f07d9f9e6..31d19dc96 100644 --- a/src/python_pybind11/src/_ignition_math_pybind11.cc +++ b/src/python_pybind11/src/_ignition_math_pybind11.cc @@ -44,6 +44,7 @@ #include "SemanticVersion.hh" #include "SignalStats.hh" #include "Sphere.hh" +#include "SphericalCoordinates.hh" #include "Spline.hh" #include "StopWatch.hh" #include "Temperature.hh" @@ -108,6 +109,9 @@ PYBIND11_MODULE(math, m) ignition::math::python::defineMathSemanticVersion(m, "SemanticVersion"); + ignition::math::python::defineMathSphericalCoordinates( + m, "SphericalCoordinates"); + ignition::math::python::defineMathSpline(m, "Spline"); ignition::math::python::defineMathStopwatch(m, "Stopwatch"); diff --git a/src/python/SphericalCoordinates_TEST.py b/src/python_pybind11/test/SphericalCoordinates_TEST.py similarity index 94% rename from src/python/SphericalCoordinates_TEST.py rename to src/python_pybind11/test/SphericalCoordinates_TEST.py index 415faec01..50a52ef96 100644 --- a/src/python/SphericalCoordinates_TEST.py +++ b/src/python_pybind11/test/SphericalCoordinates_TEST.py @@ -15,8 +15,8 @@ import unittest import ignition -from ignition.math import Angle, IGN_PI, SphericalCoordinates, Vector3d - +from ignition.math import Angle, SphericalCoordinates, Vector3d +import math class TestSphericalCoordinates(unittest.TestCase): @@ -269,8 +269,8 @@ def test_distance(self): def test_bad_set_surface(self): sc = SphericalCoordinates() - sc.set_surface(2) - self.assertEqual(sc.surface(), 2) + sc.set_surface(SphericalCoordinates.SurfaceType(2)) + self.assertEqual(sc.surface(), SphericalCoordinates.SurfaceType(2)) def test_transform(self): sc = SphericalCoordinates() @@ -297,11 +297,15 @@ def test_transform(self): def test_bad_coordinate_type(self): sc = SphericalCoordinates() pos = Vector3d(1, 2, -4) - result = sc.position_transform(pos, 7, 6) + result = sc.position_transform(pos, + SphericalCoordinates.CoordinateType(7), + SphericalCoordinates.CoordinateType(6)) self.assertEqual(result, pos) - result = sc.position_transform(pos, 4, 6) + result = sc.position_transform(pos, + SphericalCoordinates.CoordinateType(4), + SphericalCoordinates.CoordinateType(6)) self.assertEqual(result, pos) @@ -317,10 +321,14 @@ def test_bad_coordinate_type(self): SphericalCoordinates.SPHERICAL) self.assertEqual(result, pos) - result = sc.velocity_transform(pos, 7, SphericalCoordinates.ECEF) + result = sc.velocity_transform(pos, + SphericalCoordinates.CoordinateType(7), + SphericalCoordinates.ECEF) self.assertEqual(result, pos) - result = sc.velocity_transform(pos, SphericalCoordinates.ECEF, 7) + result = sc.velocity_transform(pos, + SphericalCoordinates.ECEF, + SphericalCoordinates.CoordinateType(7)) self.assertEqual(result, pos) def test_equality_ops(self): @@ -363,8 +371,8 @@ def test_assigment_op(self): def test_no_heading(self): # Default heading st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * IGN_PI / 180.0) - lon = Angle(-43.2 * IGN_PI / 180.0) + lat = Angle(-22.9 * math.pi / 180.0) + lon = Angle(-43.2 * math.pi / 180.0) heading = Angle(0.0) elev = 0 sc = SphericalCoordinates(st, lat, lon, elev, heading) @@ -446,9 +454,9 @@ def test_no_heading(self): def test_with_heading(self): # Heading 90 deg: X == North, Y == West , Z == Up st = SphericalCoordinates.EARTH_WGS84 - lat = Angle(-22.9 * IGN_PI / 180.0) - lon = Angle(-43.2 * IGN_PI / 180.0) - heading = Angle(90.0 * IGN_PI / 180.0) + lat = Angle(-22.9 * math.pi / 180.0) + lon = Angle(-43.2 * math.pi / 180.0) + heading = Angle(90.0 * math.pi / 180.0) elev = 0 sc = SphericalCoordinates(st, lat, lon, elev, heading)