From 3c6a24253df45f05372c87081c0a3b430184ef86 Mon Sep 17 00:00:00 2001 From: domire8 <71256590+domire8@users.noreply.github.com> Date: Fri, 24 Jun 2022 18:49:19 +0200 Subject: [PATCH] Set default values of empty Parameters correctly (#298) * Add virtual destructor for State * Fix constructor and ostream operator in Ellipsoid * Default construct value of parameters if not provided * Add empty constructor for parameter of ellipsoid * Update C++ tests for parameters * Add missing Shape and Ellipsoid constructors in Python * Default construct values of parameters in Python * Update Python parameter tests * Fix incorrect bracket * 6.0.1 -> 6.0.2 * Update CHANGELOG --- CHANGELOG.md | 1 + VERSION | 2 +- doxygen/doxygen.conf | 2 +- protocol/clproto_cpp/CMakeLists.txt | 2 +- python/include/parameter_container.h | 20 +- python/setup.py | 2 +- python/source/common/parameter_container.cpp | 25 +- .../state_representation/bind_geometry.cpp | 2 + .../state_representation/test_parameters.py | 27 +- source/CMakeLists.txt | 2 +- .../include/state_representation/State.hpp | 5 + .../src/geometry/Ellipsoid.cpp | 18 +- .../src/geometry/Shape.cpp | 8 +- .../src/parameters/Parameter.cpp | 33 +- .../test/tests/test_parameter.cpp | 320 +++++++++++++----- 15 files changed, 348 insertions(+), 121 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index b234a7242..95e251421 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -15,6 +15,7 @@ Release Versions: ## Upcoming changes (in development) - Extend clproto tests (#297) +- Set default values of empty Parameters correctly (#298) ## 6.0.0 diff --git a/VERSION b/VERSION index 6d54bbd77..7a9f89d81 100644 --- a/VERSION +++ b/VERSION @@ -1 +1 @@ -6.0.1 \ No newline at end of file +6.0.2 \ No newline at end of file diff --git a/doxygen/doxygen.conf b/doxygen/doxygen.conf index 4645e1f53..9ecb77d7e 100644 --- a/doxygen/doxygen.conf +++ b/doxygen/doxygen.conf @@ -38,7 +38,7 @@ PROJECT_NAME = "Control Libraries" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 6.0.1 +PROJECT_NUMBER = 6.0.2 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a diff --git a/protocol/clproto_cpp/CMakeLists.txt b/protocol/clproto_cpp/CMakeLists.txt index 42660c8e5..4ea966727 100644 --- a/protocol/clproto_cpp/CMakeLists.txt +++ b/protocol/clproto_cpp/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(clproto VERSION 6.0.1) +project(clproto VERSION 6.0.2) # Default to C99 if(NOT CMAKE_C_STANDARD) diff --git a/python/include/parameter_container.h b/python/include/parameter_container.h index 10b6c522f..b4370f7fe 100644 --- a/python/include/parameter_container.h +++ b/python/include/parameter_container.h @@ -9,17 +9,17 @@ namespace py_parameter { struct ParameterValues { - int int_value; - std::vector int_array_value; - double double_value; - std::vector double_array_value; - bool bool_value; - std::vector bool_array_value; - std::string string_value; - std::vector string_array_value; + int int_value = int(); + std::vector int_array_value = std::vector(); + double double_value = double(); + std::vector double_array_value = std::vector(); + bool bool_value = bool(); + std::vector bool_array_value= std::vector(); + std::string string_value = std::string(); + std::vector string_array_value = std::vector(); std::shared_ptr state_pointer; - Eigen::MatrixXd matrix_value; - Eigen::VectorXd vector_value; + Eigen::MatrixXd matrix_value = Eigen::MatrixXd(); + Eigen::VectorXd vector_value = Eigen::VectorXd(); }; class ParameterContainer : public ParameterInterface { diff --git a/python/setup.py b/python/setup.py index fe5ab0307..c45701650 100644 --- a/python/setup.py +++ b/python/setup.py @@ -10,7 +10,7 @@ osqp_path_var = 'OSQP_INCLUDE_DIR' openrobots_path_var = 'OPENROBOTS_INCLUDE_DIR' -__version__ = "6.0.1" +__version__ = "6.0.2" __libraries__ = ['state_representation', 'clproto', 'controllers', 'dynamical_systems', 'robot_model'] __include_dirs__ = ['include'] diff --git a/python/source/common/parameter_container.cpp b/python/source/common/parameter_container.cpp index 7b33d8fe3..087d544f1 100644 --- a/python/source/common/parameter_container.cpp +++ b/python/source/common/parameter_container.cpp @@ -10,7 +10,30 @@ namespace py_parameter { ParameterContainer::ParameterContainer( const std::string& name, const ParameterType& type, const StateType& parameter_state_type -) : ParameterInterface(name, type, parameter_state_type) {} +) : ParameterInterface(name, type, parameter_state_type) { + if (type == ParameterType::STATE) { + switch (parameter_state_type) { + case StateType::CARTESIAN_STATE: + values.state_pointer = std::make_shared(); + break; + case StateType::CARTESIAN_POSE: + values.state_pointer = std::make_shared(); + break; + case StateType::JOINT_STATE: + values.state_pointer = std::make_shared(); + break; + case StateType::JOINT_POSITIONS: + values.state_pointer = std::make_shared(); + break; + case StateType::GEOMETRY_ELLIPSOID: + values.state_pointer = std::make_shared(); + break; + default: + throw std::invalid_argument("The desired StateType for parameter " + this->get_name() + " is not supported."); + break; + } + } +} ParameterContainer::ParameterContainer( const std::string& name, const py::object& value, const ParameterType& type, const StateType& parameter_state_type diff --git a/python/source/state_representation/bind_geometry.cpp b/python/source/state_representation/bind_geometry.cpp index 6c22d3801..f3df2af04 100644 --- a/python/source/state_representation/bind_geometry.cpp +++ b/python/source/state_representation/bind_geometry.cpp @@ -8,6 +8,7 @@ void shape(py::module_& m) { py::class_, State> c(m, "Shape"); + c.def(py::init(), "Constructor with a type", "type"_a); c.def(py::init(), "Constructor with name but empty state.", "type"_a, "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor from another Shape.", "shape"_a); @@ -38,6 +39,7 @@ void shape(py::module_& m) { void ellipsoid(py::module_& m) { py::class_, Shape> c(m, "Ellipsoid"); + c.def(py::init(), "Empty constructor."); c.def(py::init(), "Constructor with name but empty state.", "name"_a, "reference_frame"_a=std::string("world")); c.def(py::init(), "Copy constructor from another Ellipsoid.", "ellipsoid"_a); diff --git a/python/test/state_representation/test_parameters.py b/python/test/state_representation/test_parameters.py index 89f25ce0d..05d66badf 100755 --- a/python/test/state_representation/test_parameters.py +++ b/python/test/state_representation/test_parameters.py @@ -42,6 +42,7 @@ def test_param_int(self): self.assertEqual(param.get_name(), "int") self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), 0) param.set_value(1) self.assertEqual(param.get_value(), 1) param1 = sr.Parameter("int", 1, sr.ParameterType.INT) @@ -53,6 +54,7 @@ def test_param_int_array(self): self.assertEqual(param.get_name(), "int_array") self.assertEqual(param.get_parameter_type(), sr.ParameterType.INT_ARRAY) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), []) values = [2, 3, 4] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] @@ -65,6 +67,7 @@ def test_param_double(self): self.assertEqual(param.get_name(), "double") self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), 0.0) param.set_value(1.5) self.assertEqual(param.get_value(), 1.5) param1 = sr.Parameter("double", 1.5, sr.ParameterType.DOUBLE) @@ -76,6 +79,7 @@ def test_param_double_array(self): self.assertEqual(param.get_name(), "double_array") self.assertEqual(param.get_parameter_type(), sr.ParameterType.DOUBLE_ARRAY) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), []) values = [2.2, 3.3, 4.4] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] @@ -88,10 +92,11 @@ def test_param_bool(self): self.assertEqual(param.get_name(), "bool") self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) - param.set_value(False) self.assertEqual(param.get_value(), False) - param1 = sr.Parameter("bool", False, sr.ParameterType.BOOL) - self.assertEqual(param1.get_value(), False) + param.set_value(True) + self.assertEqual(param.get_value(), True) + param1 = sr.Parameter("bool", True, sr.ParameterType.BOOL) + self.assertEqual(param1.get_value(), True) def test_param_bool_array(self): param = sr.Parameter("bool_array", sr.ParameterType.BOOL_ARRAY) @@ -99,6 +104,7 @@ def test_param_bool_array(self): self.assertEqual(param.get_name(), "bool_array") self.assertEqual(param.get_parameter_type(), sr.ParameterType.BOOL_ARRAY) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), []) values = [True, False, False] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] @@ -111,6 +117,7 @@ def test_param_string(self): self.assertEqual(param.get_name(), "string") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), "") param.set_value("parameter") self.assertEqual(param.get_value(), "parameter") param1 = sr.Parameter("string", "parameter", sr.ParameterType.STRING) @@ -122,6 +129,7 @@ def test_param_string_array(self): self.assertEqual(param.get_name(), "string_array") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STRING_ARRAY) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assertEqual(param.get_value(), []) values = ["test", "parameter", "bindings"] param.set_value(values) [self.assertEqual(param.get_value()[i], values[i]) for i in range(len(values))] @@ -134,6 +142,8 @@ def test_param_cartesian_state(self): self.assertEqual(param.get_name(), "cartesian_state") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_STATE) + self.assertTrue(param.get_value().is_empty()) + self.assertEqual(param.get_value().get_type(), sr.StateType.CARTESIAN_STATE) values = sr.CartesianState.Random("test") param.set_value(values) self.cartesian_equal(param.get_value(), values) @@ -146,6 +156,8 @@ def test_param_cartesian_pose(self): self.assertEqual(param.get_name(), "cartesian_pose") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.CARTESIAN_POSE) + self.assertTrue(param.get_value().is_empty()) + self.assertEqual(param.get_value().get_type(), sr.StateType.CARTESIAN_POSE) values = sr.CartesianPose.Random("test") param.set_value(values) self.cartesian_equal(param.get_value(), values) @@ -158,6 +170,8 @@ def test_param_joint_state(self): self.assertEqual(param.get_name(), "joint_state") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_STATE) + self.assertTrue(param.get_value().is_empty()) + self.assertEqual(param.get_value().get_type(), sr.StateType.JOINT_STATE) values = sr.JointState.Random("test", 3) param.set_value(values) self.joint_equal(param.get_value(), values) @@ -170,6 +184,9 @@ def test_param_joint_positions(self): self.assertEqual(param.get_name(), "joint_positions") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.JOINT_POSITIONS) + print(param.get_value()) + self.assertTrue(param.get_value().is_empty()) + self.assertEqual(param.get_value().get_type(), sr.StateType.JOINT_POSITIONS) values = sr.JointPositions.Random("test", 3) param.set_value(values) self.joint_equal(param.get_value(), values) @@ -182,6 +199,8 @@ def test_param_ellipsoid(self): self.assertEqual(param.get_name(), "ellipse") self.assertEqual(param.get_parameter_type(), sr.ParameterType.STATE) self.assertEqual(param.get_parameter_state_type(), sr.StateType.GEOMETRY_ELLIPSOID) + self.assertTrue(param.get_value().is_empty()) + self.assertEqual(param.get_value().get_type(), sr.StateType.GEOMETRY_ELLIPSOID) values = sr.Ellipsoid("test") param.set_value(values) self.assertTrue(param.get_value().get_name(), values.get_name()) @@ -194,6 +213,7 @@ def test_param_matrix(self): self.assertEqual(param.get_name(), "matrix") self.assertEqual(param.get_parameter_type(), sr.ParameterType.MATRIX) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assert_np_array_equal(param.get_value(), np.empty((0, 0))) values = np.random.rand(3, 2) param.set_value(values) self.assert_np_array_equal(param.get_value(), values) @@ -206,6 +226,7 @@ def test_param_vector(self): self.assertEqual(param.get_name(), "vector") self.assertEqual(param.get_parameter_type(), sr.ParameterType.VECTOR) self.assertEqual(param.get_parameter_state_type(), sr.StateType.NONE) + self.assert_np_array_equal(param.get_value(), np.empty(0)) values = np.random.rand(3) param.set_value(values) self.assert_np_array_equal(param.get_value(), values) diff --git a/source/CMakeLists.txt b/source/CMakeLists.txt index f9b925307..60d50bf77 100644 --- a/source/CMakeLists.txt +++ b/source/CMakeLists.txt @@ -1,6 +1,6 @@ cmake_minimum_required(VERSION 3.15) -project(control_libraries VERSION 6.0.1) +project(control_libraries VERSION 6.0.2) # Build options option(BUILD_TESTING "Build all tests." OFF) diff --git a/source/state_representation/include/state_representation/State.hpp b/source/state_representation/include/state_representation/State.hpp index 49a32f7fd..015f578f3 100644 --- a/source/state_representation/include/state_representation/State.hpp +++ b/source/state_representation/include/state_representation/State.hpp @@ -48,6 +48,11 @@ class State : public std::enable_shared_from_this { */ State(const State& state); + /** + * @brief Virtual destructor + */ + virtual ~State() = default; + /** * @brief Swap the values of the two States * @param state1 State to be swapped with 2 diff --git a/source/state_representation/src/geometry/Ellipsoid.cpp b/source/state_representation/src/geometry/Ellipsoid.cpp index 072c17057..5d9481145 100644 --- a/source/state_representation/src/geometry/Ellipsoid.cpp +++ b/source/state_representation/src/geometry/Ellipsoid.cpp @@ -13,9 +13,7 @@ Ellipsoid::Ellipsoid(const std::string& name, const std::string& reference_frame } Ellipsoid::Ellipsoid(const Ellipsoid& ellipsoid) : - Shape(ellipsoid), axis_lengths_(ellipsoid.axis_lengths_), rotation_angle_(0) { - this->set_filled(); -} + Shape(ellipsoid), axis_lengths_(ellipsoid.axis_lengths_), rotation_angle_(0) {} const std::list Ellipsoid::sample_from_parameterization(unsigned int nb_samples) const { if (this->get_center_state().is_empty()) { @@ -185,11 +183,15 @@ void Ellipsoid::set_data(const std::vector& data) { } std::ostream& operator<<(std::ostream& os, const Ellipsoid& ellipsoid) { - os << "Ellipsoid " << ellipsoid.get_name() << " of dimensions ["; - os << ellipsoid.get_axis_length(0) << ", "; - os << ellipsoid.get_axis_length(1) << "]"; - os << " with state:" << std::endl; - os << ellipsoid.get_center_state(); + if (ellipsoid.is_empty()) { + os << "Empty Ellipsoid"; + } else { + os << "Ellipsoid " << ellipsoid.get_name() << " of dimensions ["; + os << ellipsoid.get_axis_length(0) << ", "; + os << ellipsoid.get_axis_length(1) << "]"; + os << " with state:" << std::endl; + os << ellipsoid.get_center_state(); + } return os; } } \ No newline at end of file diff --git a/source/state_representation/src/geometry/Shape.cpp b/source/state_representation/src/geometry/Shape.cpp index 265276a0e..7ed773c17 100644 --- a/source/state_representation/src/geometry/Shape.cpp +++ b/source/state_representation/src/geometry/Shape.cpp @@ -10,8 +10,12 @@ Shape::Shape(const StateType& type, const std::string& name, const std::string& Shape::Shape(const Shape& shape) : State(shape), center_state_(shape.center_state_) {} std::ostream& operator<<(std::ostream& os, const Shape& shape) { - os << "Shape " << shape.get_name() << " with state:" << std::endl; - os << shape.get_center_state(); + if (shape.is_empty()) { + os << "Empty Shape"; + } else { + os << "Shape " << shape.get_name() << " with state:" << std::endl; + os << shape.get_center_state(); + } return os; } } \ No newline at end of file diff --git a/source/state_representation/src/parameters/Parameter.cpp b/source/state_representation/src/parameters/Parameter.cpp index b0c13f0bb..b067ca9fe 100644 --- a/source/state_representation/src/parameters/Parameter.cpp +++ b/source/state_representation/src/parameters/Parameter.cpp @@ -8,7 +8,7 @@ namespace state_representation { template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::INT) {} + ParameterInterface(name, ParameterType::INT), value_() {} template<> Parameter::Parameter(const std::string& name, const int& value) : @@ -18,7 +18,7 @@ Parameter::Parameter(const std::string& name, const int& value) : template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::INT_ARRAY) {} + ParameterInterface(name, ParameterType::INT_ARRAY), value_() {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : @@ -28,7 +28,7 @@ Parameter>::Parameter(const std::string& name, const std::vecto template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::DOUBLE) {} + ParameterInterface(name, ParameterType::DOUBLE), value_() {} template<> Parameter::Parameter(const std::string& name, const double& value) : @@ -38,7 +38,7 @@ Parameter::Parameter(const std::string& name, const double& value) : template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::DOUBLE_ARRAY) {} + ParameterInterface(name, ParameterType::DOUBLE_ARRAY), value_() {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : @@ -48,7 +48,7 @@ Parameter>::Parameter(const std::string& name, const std::ve template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::BOOL) {} + ParameterInterface(name, ParameterType::BOOL), value_() {} template<> Parameter::Parameter(const std::string& name, const bool& value) : @@ -58,7 +58,7 @@ Parameter::Parameter(const std::string& name, const bool& value) : template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::BOOL_ARRAY) { + ParameterInterface(name, ParameterType::BOOL_ARRAY), value_() { } template<> @@ -69,7 +69,7 @@ Parameter>::Parameter(const std::string& name, const std::vect template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STRING) {} + ParameterInterface(name, ParameterType::STRING), value_() {} template<> Parameter::Parameter(const std::string& name, const std::string& value) : @@ -79,7 +79,7 @@ Parameter::Parameter(const std::string& name, const std::string& va template<> Parameter>::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STRING_ARRAY) {} + ParameterInterface(name, ParameterType::STRING_ARRAY), value_() {} template<> Parameter>::Parameter(const std::string& name, const std::vector& value) : @@ -89,7 +89,7 @@ Parameter>::Parameter(const std::string& name, const st template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE) {} + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_STATE), value_() {} template<> Parameter::Parameter(const std::string& name, const CartesianState& value) : @@ -99,7 +99,7 @@ Parameter::Parameter(const std::string& name, const CartesianSta template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE) {} + ParameterInterface(name, ParameterType::STATE, StateType::CARTESIAN_POSE), value_() {} template<> Parameter::Parameter(const std::string& name, const CartesianPose& value) : @@ -109,7 +109,7 @@ Parameter::Parameter(const std::string& name, const CartesianPose template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE) {} + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_STATE), value_() {} template<> Parameter::Parameter(const std::string& name, const JointState& value) : @@ -119,7 +119,7 @@ Parameter::Parameter(const std::string& name, const JointState& valu template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS) {} + ParameterInterface(name, ParameterType::STATE, StateType::JOINT_POSITIONS), value_() {} template<> Parameter::Parameter(const std::string& name, const JointPositions& value) : @@ -127,6 +127,11 @@ Parameter::Parameter(const std::string& name, const JointPositio this->set_filled(); } +template<> +Parameter::Parameter(const std::string& name) : + ParameterInterface(name, ParameterType::STATE, StateType::GEOMETRY_ELLIPSOID), value_() { +} + template<> Parameter::Parameter(const std::string& name, const Ellipsoid& value) : ParameterInterface(name, ParameterType::STATE, StateType::GEOMETRY_ELLIPSOID), value_(value) { @@ -135,7 +140,7 @@ Parameter::Parameter(const std::string& name, const Ellipsoid& value) template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::MATRIX) {} + ParameterInterface(name, ParameterType::MATRIX), value_() {} template<> Parameter::Parameter(const std::string& name, const Eigen::MatrixXd& value) : @@ -145,7 +150,7 @@ Parameter::Parameter(const std::string& name, const Eigen::Matr template<> Parameter::Parameter(const std::string& name) : - ParameterInterface(name, ParameterType::VECTOR) {} + ParameterInterface(name, ParameterType::VECTOR), value_() {} template<> Parameter::Parameter(const std::string& name, const Eigen::VectorXd& value) : diff --git a/source/state_representation/test/tests/test_parameter.cpp b/source/state_representation/test/tests/test_parameter.cpp index 03f8a1a04..dc6e4fe27 100644 --- a/source/state_representation/test/tests/test_parameter.cpp +++ b/source/state_representation/test/tests/test_parameter.cpp @@ -1,7 +1,8 @@ +#include "state_representation/geometry/Ellipsoid.hpp" #include "state_representation/parameters/Event.hpp" #include "state_representation/parameters/Parameter.hpp" #include "state_representation/space/cartesian/CartesianPose.hpp" -#include "state_representation/space/cartesian/CartesianState.hpp" +#include "state_representation/space/joint/JointPositions.hpp" #include "state_representation/exceptions/InvalidParameterCastException.hpp" #include "state_representation/exceptions/InvalidPointerException.hpp" @@ -10,10 +11,139 @@ using namespace state_representation; -TEST(ParameterTest, CopyConstructor) { - Parameter int_param("test", 1); - ParameterInterface int_param_interface(int_param); - EXPECT_EQ(int_param_interface.get_parameter_type(), int_param.get_parameter_type()); +template using ParamT = std::vector>; + +static std::tuple, + ParamT>, + ParamT, + ParamT>, + ParamT, + ParamT>, + ParamT, + ParamT>, + ParamT, + ParamT, + ParamT, + ParamT, + ParamT, + ParamT, + ParamT> + parameter_test_cases{{std::make_tuple(false, ParameterType::BOOL, StateType::NONE)}, { + std::make_tuple(std::vector({true, false, true}), ParameterType::BOOL_ARRAY, StateType::NONE) +}, {std::make_tuple(1, ParameterType::INT, StateType::NONE)}, { + std::make_tuple(std::vector({1, 2, 3}), ParameterType::INT_ARRAY, StateType::NONE) + }, {std::make_tuple(1.0, ParameterType::DOUBLE, StateType::NONE)}, { + std::make_tuple( + std::vector({1.0, 2.0, 3.0}), ParameterType::DOUBLE_ARRAY, StateType::NONE + ) + }, { + std::make_tuple("test", ParameterType::STRING, StateType::NONE) + }, { + std::make_tuple( + std::vector({"1", "2", "3"}), ParameterType::STRING_ARRAY, StateType::NONE + ) + }, { + std::make_tuple( + CartesianState::Random("test", "base"), ParameterType::STATE, + StateType::CARTESIAN_STATE + ) + }, { + std::make_tuple( + CartesianPose::Random("test", "base"), ParameterType::STATE, StateType::CARTESIAN_POSE + ) + }, { + std::make_tuple( + JointState::Random("test", 3), ParameterType::STATE, StateType::JOINT_STATE + ) + }, { + std::make_tuple( + JointPositions::Random("test", 3), ParameterType::STATE, StateType::JOINT_POSITIONS + ) + }, { + std::make_tuple(Ellipsoid("test"), ParameterType::STATE, StateType::GEOMETRY_ELLIPSOID) + }, { + std::make_tuple(Eigen::VectorXd::Random(2), ParameterType::VECTOR, StateType::NONE) + }, { + std::make_tuple(Eigen::MatrixXd::Random(2, 2), ParameterType::MATRIX, StateType::NONE) + }}; + +template +void expect_values_equal(const T& value_1, const T& value_2) { + EXPECT_EQ(value_1, value_2); +} + +template<> +void expect_values_equal(const CartesianState& value_1, const CartesianState& value_2) { + EXPECT_EQ(value_1.get_name(), value_2.get_name()); + EXPECT_EQ(value_1.get_reference_frame(), value_2.get_reference_frame()); + EXPECT_TRUE(value_1.data().isApprox(value_2.data())); +} + +template<> +void expect_values_equal(const CartesianPose& value_1, const CartesianPose& value_2) { + EXPECT_EQ(value_1.get_name(), value_2.get_name()); + EXPECT_EQ(value_1.get_reference_frame(), value_2.get_reference_frame()); + EXPECT_TRUE(value_1.data().isApprox(value_2.data())); +} + +template<> +void expect_values_equal(const JointState& value_1, const JointState& value_2) { + EXPECT_EQ(value_1.get_name(), value_2.get_name()); + ASSERT_EQ(value_1.get_size(), value_2.get_size()); + for (std::size_t i = 0; i < value_1.get_size(); ++i) { + EXPECT_EQ(value_1.get_names().at(i), value_2.get_names().at(i)); + } + EXPECT_TRUE(value_1.data().isApprox(value_2.data())); +} + +template<> +void expect_values_equal(const JointPositions& value_1, const JointPositions& value_2) { + EXPECT_EQ(value_1.get_name(), value_2.get_name()); + ASSERT_EQ(value_1.get_size(), value_2.get_size()); + for (std::size_t i = 0; i < value_1.get_size(); ++i) { + EXPECT_EQ(value_1.get_names().at(i), value_2.get_names().at(i)); + } + EXPECT_TRUE(value_1.data().isApprox(value_2.data())); +} + +template<> +void expect_values_equal(const Ellipsoid& value_1, const Ellipsoid& value_2) { + EXPECT_EQ(value_1.get_name(), value_2.get_name()); + ASSERT_EQ(value_1.get_axis_lengths().size(), value_2.get_axis_lengths().size()); + for (std::size_t i = 0; i < value_1.get_axis_lengths().size(); ++i) { + EXPECT_EQ(value_1.get_axis_length(i), value_1.get_axis_length(i)); + } + EXPECT_EQ(value_1.get_center_pose().get_name(), value_2.get_center_pose().get_name()); + EXPECT_EQ(value_1.get_center_pose().get_reference_frame(), value_2.get_center_pose().get_reference_frame()); + EXPECT_TRUE(value_1.get_center_pose().data().isApprox(value_2.get_center_pose().data())); +} + +template +class ParameterTest : public testing::Test { +public: + ParameterTest() : test_cases_{std::get>(parameter_test_cases)} {} +protected: + ParamT test_cases_; +}; +TYPED_TEST_SUITE_P(ParameterTest); + +TYPED_TEST_P(ParameterTest, Construction) { + for (auto const& test_case: this->test_cases_) { + Parameter param("test"); + EXPECT_EQ(param.get_name(), "test"); + EXPECT_EQ(param.get_parameter_type(), std::get<1>(test_case)); + EXPECT_EQ(param.get_parameter_state_type(), std::get<2>(test_case)); + EXPECT_TRUE(param.is_empty()); + expect_values_equal(param.get_value(), TypeParam()); + ParameterInterface param_interface(param); + EXPECT_EQ(param_interface.get_name(), param.get_name()); + EXPECT_EQ(param_interface.get_type(), StateType::PARAMETER); + EXPECT_EQ(param_interface.get_parameter_type(), param.get_parameter_type()); + EXPECT_EQ(param_interface.get_parameter_state_type(), param.get_parameter_state_type()); + param.set_value(std::get<0>(test_case)); + EXPECT_FALSE(param.is_empty()); + expect_values_equal(param.get_value(), std::get<0>(test_case)); + } } TEST(ParameterTest, Conversion) { @@ -95,88 +225,122 @@ TEST(ParameterTest, Event) { EXPECT_TRUE(ptr_e->get_value()); } -TEST(ParameterTest, MakeShared) { - auto pose = CartesianPose::Random("A", "B"); - auto param = make_shared_parameter("name", pose); - EXPECT_EQ(param->get_name(), "name"); - EXPECT_EQ(param->get_type(), StateType::PARAMETER); - EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE); - EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE); - EXPECT_EQ(param->is_empty(), false); - EXPECT_EQ(param->get_value().get_name(), "A"); - EXPECT_EQ(param->get_value().get_reference_frame(), "B"); - EXPECT_TRUE(param->get_value().data().isApprox(pose.data())); +TYPED_TEST_P(ParameterTest, MakeShared) { + for (auto const& test_case: this->test_cases_) { + auto param = make_shared_parameter("test", std::get<0>(test_case)); + EXPECT_EQ(param->get_name(), "test"); + EXPECT_EQ(param->get_type(), StateType::PARAMETER); + EXPECT_FALSE(param->is_empty()); + EXPECT_EQ(param->get_parameter_type(), std::get<1>(test_case)); + EXPECT_EQ(param->get_parameter_state_type(), std::get<2>(test_case)); + expect_values_equal(param->get_value(), std::get<0>(test_case)); + } } -TEST(ParameterTest, ParameterThroughInterface) { - auto pose = CartesianPose::Random("A", "B"); - std::shared_ptr param_interface = make_shared_parameter("name", pose); - - auto param = param_interface->get_parameter(); - EXPECT_EQ(param->get_name(), "name"); - EXPECT_EQ(param->get_type(), StateType::PARAMETER); - EXPECT_EQ(param->get_parameter_type(), ParameterType::STATE); - EXPECT_EQ(param->get_parameter_state_type(), StateType::CARTESIAN_POSE); - EXPECT_EQ(param->is_empty(), false); - EXPECT_EQ(param->get_value().get_name(), "A"); - EXPECT_EQ(param->get_value().get_reference_frame(), "B"); - EXPECT_TRUE(param->get_value().data().isApprox(pose.data())); - - auto param_value = param_interface->get_parameter_value(); - EXPECT_EQ(param_value.get_type(), StateType::CARTESIAN_POSE); - EXPECT_EQ(param_value.get_name(), "A"); - EXPECT_EQ(param_value.get_reference_frame(), "B"); - EXPECT_TRUE(param_value.data().isApprox(pose.data())); - - auto pose2 = CartesianPose::Random("C", "D"); - param_interface->set_parameter_value(pose2); - param_value = param_interface->get_parameter_value(); - EXPECT_EQ(param_value.get_name(), "C"); - EXPECT_EQ(param_value.get_reference_frame(), "D"); - EXPECT_TRUE(param_value.data().isApprox(pose2.data())); +TYPED_TEST_P(ParameterTest, ParameterThroughInterface) { + for (auto const& test_case: this->test_cases_) { + std::shared_ptr param_interface = make_shared_parameter("test", std::get<0>(test_case)); + + auto param = param_interface->get_parameter(); + EXPECT_EQ(param->get_name(), "test"); + EXPECT_EQ(param->get_type(), StateType::PARAMETER); + EXPECT_FALSE(param->is_empty()); + EXPECT_EQ(param->get_parameter_type(), std::get<1>(test_case)); + EXPECT_EQ(param->get_parameter_state_type(), std::get<2>(test_case)); + expect_values_equal(param->get_value(), std::get<0>(test_case)); + + auto param_value = param_interface->get_parameter_value(); + expect_values_equal(param_value, std::get<0>(test_case)); + + param_interface->set_parameter_value(TypeParam()); + param_value = param_interface->get_parameter_value(); + expect_values_equal(param_value, TypeParam()); + } } -TEST(ParameterTest, ParameterInterfaceBadPointer) { - ParameterInterface parameter_interface("name", ParameterType::INT); +TYPED_TEST_P(ParameterTest, ParameterInterfaceBadPointer) { + for (auto const& test_case: this->test_cases_) { + ParameterInterface parameter_interface("test", std::get<1>(test_case)); - // by default (validate_pointer = true), throw when the ParameterInterface instance is not managed by any pointer - EXPECT_THROW(parameter_interface.get_parameter(), exceptions::InvalidPointerException); - EXPECT_THROW(parameter_interface.get_parameter(true), exceptions::InvalidPointerException); + // by default (validate_pointer = true), throw when the ParameterInterface instance is not managed by any pointer + EXPECT_THROW(parameter_interface.get_parameter(), exceptions::InvalidPointerException); + EXPECT_THROW(parameter_interface.get_parameter(true), exceptions::InvalidPointerException); - // using validate_pointer = false catches the exception but returns a null pointer - EXPECT_NO_THROW(parameter_interface.get_parameter(false)); - EXPECT_EQ(parameter_interface.get_parameter(false), nullptr); + // using validate_pointer = false catches the exception but returns a null pointer + EXPECT_NO_THROW(parameter_interface.get_parameter(false)); + EXPECT_EQ(parameter_interface.get_parameter(false), nullptr); + } } -TEST(ParameterTest, ParameterInterfaceNullCast) { - auto parameter_interface_ptr = std::make_shared("name", ParameterType::INT); - std::shared_ptr> parameter; +TYPED_TEST_P(ParameterTest, ParameterInterfaceNullCast) { + for (auto const& test_case: this->test_cases_) { + auto parameter_interface_ptr = std::make_shared("test", std::get<1>(test_case)); + std::shared_ptr> parameter; + + // by default (validate_pointer = true), throw when the pointer does not address a Parameter instance + EXPECT_THROW(parameter_interface_ptr->template get_parameter(), + exceptions::InvalidParameterCastException); + EXPECT_THROW(parameter_interface_ptr->template get_parameter(true), + exceptions::InvalidParameterCastException); - // by default (validate_pointer = true), throw when the pointer does not address a Parameter instance - EXPECT_THROW(parameter_interface_ptr->get_parameter(), exceptions::InvalidParameterCastException); - EXPECT_THROW(parameter_interface_ptr->get_parameter(true), exceptions::InvalidParameterCastException); + // using validate_pointer = false catches the exception but returns a null pointer + EXPECT_NO_THROW(parameter = parameter_interface_ptr->template get_parameter(false)); + EXPECT_EQ(parameter, nullptr); + } +} - // using validate_pointer = false catches the exception but returns a null pointer - EXPECT_NO_THROW(parameter = parameter_interface_ptr->get_parameter(false)); - EXPECT_EQ(parameter, nullptr); +TYPED_TEST_P(ParameterTest, ParameterInterfaceWrongTypeCast) { + for (auto const& test_case: this->test_cases_) { + std::shared_ptr + parameter_interface_ptr = make_shared_parameter("test", std::get<0>(test_case)); + + std::shared_ptr> parameter; + EXPECT_NO_THROW(parameter = parameter_interface_ptr->get_parameter()); + EXPECT_NO_THROW(parameter_interface_ptr->get_parameter(true)); + EXPECT_NO_THROW(parameter_interface_ptr->get_parameter(false)); + EXPECT_NE(parameter, nullptr); + expect_values_equal(parameter->get_value(), std::get<0>(test_case)); + + if (std::get<1>(test_case) == ParameterType::STRING) { + std::shared_ptr> parameter_int; + EXPECT_THROW(parameter_interface_ptr->get_parameter(), exceptions::InvalidParameterCastException); + EXPECT_THROW(parameter_interface_ptr->get_parameter(true), exceptions::InvalidParameterCastException); + EXPECT_NO_THROW(parameter_int = parameter_interface_ptr->get_parameter(false)); + EXPECT_EQ(parameter_int, nullptr); + + EXPECT_NO_THROW(parameter_interface_ptr->get_parameter_value()); + EXPECT_THROW(parameter_interface_ptr->get_parameter_value(), exceptions::InvalidParameterCastException); + } else { + std::shared_ptr> parameter_string; + EXPECT_THROW(parameter_interface_ptr->get_parameter(), exceptions::InvalidParameterCastException); + EXPECT_THROW(parameter_interface_ptr->get_parameter(true), + exceptions::InvalidParameterCastException); + EXPECT_NO_THROW(parameter_string = parameter_interface_ptr->get_parameter(false)); + EXPECT_EQ(parameter_string, nullptr); + + EXPECT_NO_THROW(parameter_interface_ptr->get_parameter_value()); + EXPECT_THROW(parameter_interface_ptr->get_parameter_value(), + exceptions::InvalidParameterCastException); + } + } } -TEST(ParameterTest, ParameterInterfaceWrongTypeCast) { - std::shared_ptr parameter_interface_ptr = make_shared_parameter("name", 1); - - std::shared_ptr> parameter_int; - EXPECT_NO_THROW(parameter_int = parameter_interface_ptr->get_parameter()); - EXPECT_NO_THROW(parameter_interface_ptr->get_parameter(true)); - EXPECT_NO_THROW(parameter_interface_ptr->get_parameter(false)); - EXPECT_NE(parameter_int, nullptr); - EXPECT_EQ(parameter_int->get_value(), 1); - - std::shared_ptr> parameter_string; - EXPECT_THROW(parameter_interface_ptr->get_parameter(), exceptions::InvalidParameterCastException); - EXPECT_THROW(parameter_interface_ptr->get_parameter(true), exceptions::InvalidParameterCastException); - EXPECT_NO_THROW(parameter_string = parameter_interface_ptr->get_parameter(false)); - EXPECT_EQ(parameter_string, nullptr); - - EXPECT_NO_THROW(parameter_interface_ptr->get_parameter_value()); - EXPECT_THROW(parameter_interface_ptr->get_parameter_value(), exceptions::InvalidParameterCastException); -} \ No newline at end of file +REGISTER_TYPED_TEST_SUITE_P(ParameterTest, Construction, MakeShared, ParameterThroughInterface, + ParameterInterfaceBadPointer, ParameterInterfaceNullCast, ParameterInterfaceWrongTypeCast); + +using ParameterTestTypes = testing::Types, + int, + std::vector, + double, + std::vector, + std::string, + std::vector, + CartesianState, + CartesianPose, + JointPositions, + JointState, + Ellipsoid, + Eigen::VectorXd, + Eigen::MatrixXd>; +INSTANTIATE_TYPED_TEST_SUITE_P(TestPrefix, ParameterTest, ParameterTestTypes); \ No newline at end of file