From 0aa8a729963a6043f4f623e6b22718a2897eb7b2 Mon Sep 17 00:00:00 2001 From: Tyler Weaver Date: Mon, 3 Jan 2022 13:16:06 -0700 Subject: [PATCH] :test_tube: parameter tests (#10) Signed-off-by: Tyler Weaver --- .github/workflows/ci.yaml | 14 ++-- CMakeLists.txt | 1 + include/bio_ik/parameters.hpp | 11 +++ src/parameters.cpp | 33 +++++---- test/CMakeLists.txt | 7 ++ test/parameter_tests.cpp | 129 ++++++++++++++++++++++++++++++++++ 6 files changed, 169 insertions(+), 26 deletions(-) create mode 100644 test/parameter_tests.cpp diff --git a/.github/workflows/ci.yaml b/.github/workflows/ci.yaml index 4b71d1e..df15e47 100644 --- a/.github/workflows/ci.yaml +++ b/.github/workflows/ci.yaml @@ -17,14 +17,12 @@ jobs: fail-fast: false matrix: env: - # TODO(tylerjw): re-enable testing on testing once moveit is fixed on buildfarm - # - NAME: "rolling-testing-ccov" - # ROS_REPO: testing - # TODO(tylerjw): enable coverage once there is a test - # TARGET_CMAKE_ARGS: -DENABLE_COVERAGE=ON - # CCOV: true + - NAME: "rolling-testing-ccov" + ROS_REPO: testing + TARGET_CMAKE_ARGS: -DENABLE_COVERAGE=ON + CCOV: true - NAME: "rolling-main" - # ROS_REPO: main + ROS_REPO: main - NAME: "address-leak-ub-sanitizers" TARGET_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=Debug @@ -51,8 +49,6 @@ jobs: env: ROS_DISTRO: rolling - # TODO(tylerjw): re-enable testing on testing once moveit is fixed on buildfarm - ROS_REPO: main UPSTREAM_WORKSPACE: upstream.repos UPSTREAM_CMAKE_ARGS: > -DCMAKE_BUILD_TYPE=Release diff --git a/CMakeLists.txt b/CMakeLists.txt index 29db255..dfec965 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -111,6 +111,7 @@ target_link_libraries( project_warnings PUBLIC Eigen3::Eigen + fmt::fmt moveit_core::moveit_collision_detection_fcl moveit_core::moveit_kinematics_base moveit_core::moveit_robot_model diff --git a/include/bio_ik/parameters.hpp b/include/bio_ik/parameters.hpp index 0e839f2..36cc8e8 100644 --- a/include/bio_ik/parameters.hpp +++ b/include/bio_ik/parameters.hpp @@ -28,6 +28,8 @@ #pragma once +#include + #include #include #include @@ -59,6 +61,15 @@ struct RosParameters { size_t population_size = 8; size_t elite_count = 4; bool enable_linear_fitness = false; + + inline operator std::string() const { + return fmt::format( + "[RosParameters: enable_profiler={}, mode={}, enable_counter={}, " + "random_seed={}, dpos={}, drot={}, dtwist={}, skip_wipeout={}, " + "population_size={}, elite_count={}, enable_linear_fitness={}]", + enable_profiler, mode, enable_counter, random_seed, dpos, drot, dtwist, + skip_wipeout, population_size, elite_count, enable_linear_fitness); + } }; /** diff --git a/src/parameters.cpp b/src/parameters.cpp index f1c1258..db78f8f 100644 --- a/src/parameters.cpp +++ b/src/parameters.cpp @@ -136,41 +136,40 @@ std::set valid_modes() { std::optional validate(const RosParameters& ros_params) { if (valid_modes().count(ros_params.mode) == 0) - return fmt::format("Mode: {}\nValid Modes: {}\n", ros_params.mode, + return fmt::format("Mode: \"{}\" is not in set: {}\n", ros_params.mode, valid_modes()); return std::nullopt; } std::optional get_ros_parameters( const rclcpp::Node::SharedPtr& node) { - using rclcpp::ParameterValue; - RosParameters ros_params; - - auto ros_parameters = RosParameters{ + const auto default_values = RosParameters{}; + const auto ros_params = RosParameters{ .enable_profiler = - declare(node, "enable_profiler", ros_params.enable_profiler, + declare(node, "enable_profiler", default_values.enable_profiler, DescriptorBuilder().additional_constraints( fmt::format("One of {}", valid_modes()))), - .mode = declare(node, "mode", ros_params.mode), + .mode = declare(node, "mode", default_values.mode), .enable_counter = - declare(node, "enable_counter", ros_params.enable_counter), - .random_seed = declare(node, "random_seed", ros_params.random_seed), - .dpos = declare(node, "dpos", ros_params.dpos), - .drot = declare(node, "drot", ros_params.drot), - .dtwist = declare(node, "dtwist", ros_params.dtwist), - .skip_wipeout = declare(node, "skip_wipeout", ros_params.skip_wipeout), + declare(node, "enable_counter", default_values.enable_counter), + .random_seed = declare(node, "random_seed", default_values.random_seed), + .dpos = declare(node, "dpos", default_values.dpos), + .drot = declare(node, "drot", default_values.drot), + .dtwist = declare(node, "dtwist", default_values.dtwist), + .skip_wipeout = + declare(node, "skip_wipeout", default_values.skip_wipeout), .population_size = static_cast( declare(node, "population_size", - static_cast(ros_params.population_size), + static_cast(default_values.population_size), DescriptorBuilder().integer_range(2))), .elite_count = static_cast(declare( - node, "elite_count", static_cast(ros_params.elite_count), + node, "elite_count", static_cast(default_values.elite_count), DescriptorBuilder().integer_range(2))), .enable_linear_fitness = declare(node, "enable_linear_fitness", - ros_params.enable_linear_fitness), + default_values.enable_linear_fitness), }; - if (auto error = validate(ros_params)) { + if (const auto error = validate(ros_params)) { RCLCPP_ERROR_STREAM(rclcpp::get_logger("bio_ik"), *error); return std::nullopt; } else { diff --git a/test/CMakeLists.txt b/test/CMakeLists.txt index cb765a9..545a8ff 100644 --- a/test/CMakeLists.txt +++ b/test/CMakeLists.txt @@ -9,4 +9,11 @@ if(BUILD_TESTING) # project_warnings # bio_ik # ) + + ament_add_gtest(parameter_tests parameter_tests.cpp) + target_link_libraries(parameter_tests + project_options + project_warnings + bio_ik + ) endif() diff --git a/test/parameter_tests.cpp b/test/parameter_tests.cpp new file mode 100644 index 0000000..8ed26ed --- /dev/null +++ b/test/parameter_tests.cpp @@ -0,0 +1,129 @@ +// Copyright 2021 PickNik Inc +// +// Redistribution and use in source and binary forms, with or without +// modification, are permitted provided that the following conditions are met: +// +// * Redistributions of source code must retain the above copyright +// notice, this list of conditions and the following disclaimer. +// +// * Redistributions in binary form must reproduce the above copyright +// notice, this list of conditions and the following disclaimer in the +// documentation and/or other materials provided with the distribution. +// +// * Neither the name of the PickNik Inc nor the names of its +// contributors may be used to endorse or promote products derived from +// this software without specific prior written permission. +// +// THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" +// AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE +// IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE +// ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE +// LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR +// CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF +// SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS +// INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN +// CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) +// ARISING IN ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE +// POSSIBILITY OF SUCH DAMAGE. + +#include +#include +#include + +#include "bio_ik/parameters.hpp" +#include "gtest/gtest.h" + +TEST(ParameterTests, DefaultValidMode) // NOLINT +{ + // GIVEN a default constructed bio_ik::RosParameters + const auto ros_params = bio_ik::RosParameters{}; + + // WHEN we call validate + const auto result = bio_ik::validate(ros_params); + + // THEN we expect the result to not contain an error + EXPECT_FALSE(result.has_value()) + << "bio_ik::validate returned error: " << *result; +} + +TEST(ParameterTests, NotValidMode) // NOLINT +{ + // GIVEN a bio_ik::RosParameters with an invalid mode + const auto ros_params = bio_ik::RosParameters{.mode = "invalid"}; + + // WHEN we call validate + const auto result = bio_ik::validate(ros_params); + + // THEN we expect the result to contain an error + EXPECT_TRUE(result.has_value()) << "bio_ik::validate did not return an error"; +} + +TEST(ParameterTests, GetIsValid) // NOLIINT +{ + // WHEN we call get_ros_parameters with no overridden parameters + const auto ros_params = + bio_ik::get_ros_parameters(std::make_shared("_")); + + // THEN we expect the result to be valid + EXPECT_TRUE(ros_params.has_value()) + << "get_ros_parameters did not return a result"; +} + +TEST(ParameterTests, DefaultSameAsRosDefault) // NOLINT +{ + // GIVEN a default constructed bio_ik::RosParameters + const auto default_ros_parameters = bio_ik::RosParameters{}; + + // WHEN we create another one using the get_ros_parameters function without + // overriding any ros parameters + const auto ros_params = + bio_ik::get_ros_parameters(std::make_shared("_")); + + // THEN we expect the result to have a value and that to be the same as the + // default constructed one except for the random seed + ASSERT_TRUE(ros_params.has_value()) + << "get_ros_parameters did not return a result"; + EXPECT_TRUE([](const auto& lhs, const auto& rhs) { + return std::tie(lhs.enable_profiler, lhs.mode, lhs.enable_counter, lhs.dpos, + lhs.drot, lhs.dtwist, lhs.skip_wipeout, lhs.population_size, + lhs.elite_count, lhs.enable_linear_fitness) == + std::tie(rhs.enable_profiler, rhs.mode, rhs.enable_counter, rhs.dpos, + rhs.drot, rhs.dtwist, rhs.skip_wipeout, rhs.population_size, + rhs.elite_count, rhs.enable_linear_fitness); + }(ros_params.value(), default_ros_parameters)) + << "\nget_ros_parameters: " + std::string(ros_params.value()) + + "\ndefault_ros_parameters: " + std::string(default_ros_parameters); +} + +TEST(ParameterTests, StringOperator) // NOLINT +{ + // GIVEN a default constructed bio_ik::RosParameters + // WHEN we convert it to std::string + // THEN we don't expect it to not throw + EXPECT_NO_THROW(auto _ = std::string(bio_ik::RosParameters{})); +} + +TEST(ParameterTests, GetInvalidParameters) // NOLINT +{ + // GIVEN a ros node with a parameter override + auto node = std::make_shared( + "_", rclcpp::NodeOptions().append_parameter_override( + "mode", rclcpp::ParameterValue("invalid"))); + + // WHEN we call get_ros_parameters + auto result = bio_ik::get_ros_parameters(node); + + // THEN we expect the result to have failed + EXPECT_FALSE(result.has_value()) + << "get_ros_parameters should have returned no value, returned: " + + std::string(result.value()); +} + +int main(int argc, char** argv) { + rclcpp::init(argc, argv); + ::testing::InitGoogleTest(&argc, argv); + + int result = RUN_ALL_TESTS(); + rclcpp::shutdown(); + return result; +}