Skip to content

Commit

Permalink
Added Vector3Stats pybind11 interface (#351)
Browse files Browse the repository at this point in the history
Signed-off-by: Alejandro Hernández <ahcorde@gmail.com>

Co-authored-by: Louise Poubel <louise@openrobotics.org>
  • Loading branch information
ahcorde and chapulina committed Dec 30, 2021
1 parent 5fa6f78 commit 4a9374f
Show file tree
Hide file tree
Showing 6 changed files with 155 additions and 29 deletions.
1 change: 0 additions & 1 deletion src/python/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -89,7 +89,6 @@ if (PYTHONLIBS_FOUND)
set(python_tests
python_TEST
SphericalCoordinates_TEST
Vector3Stats_TEST
)

foreach (test ${python_tests})
Expand Down
2 changes: 2 additions & 0 deletions src/python_pybind11/CMakeLists.txt
Original file line number Diff line number Diff line change
Expand Up @@ -32,6 +32,7 @@ if (${pybind11_FOUND})
src/Spline.cc
src/StopWatch.cc
src/Temperature.cc
src/Vector3Stats.cc
)

target_link_libraries(math PRIVATE
Expand Down Expand Up @@ -121,6 +122,7 @@ if (${pybind11_FOUND})
Triangle_TEST
Vector2_TEST
Vector3_TEST
Vector3Stats_TEST
Vector4_TEST
)

Expand Down
78 changes: 78 additions & 0 deletions src/python_pybind11/src/Vector3Stats.cc
Original file line number Diff line number Diff line change
@@ -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 <string>

#include <ignition/math/Vector3Stats.hh>

#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_<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
43 changes: 43 additions & 0 deletions src/python_pybind11/src/Vector3Stats.hh
Original file line number Diff line number Diff line change
@@ -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 <string>

#include <pybind11/pybind11.h>

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_
3 changes: 3 additions & 0 deletions src/python_pybind11/src/_ignition_math_pybind11.cc
Original file line number Diff line number Diff line change
Expand Up @@ -51,6 +51,7 @@
#include "Triangle3.hh"
#include "Vector2.hh"
#include "Vector3.hh"
#include "Vector3Stats.hh"
#include "Vector4.hh"

namespace py = pybind11;
Expand Down Expand Up @@ -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");
Expand Down
Original file line number Diff line number Diff line change
Expand Up @@ -36,57 +36,58 @@ 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)
self.assertAlmostEqual(v3stats.mag().count(), 0)

# 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)
self.assertAlmostEqual(v3stats.mag().count(), 0)

# 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)
Expand All @@ -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))
Expand Down

0 comments on commit 4a9374f

Please sign in to comment.