From f70c7ef91236097077fb79a413b622760a0bd96e Mon Sep 17 00:00:00 2001 From: Tom Moore Date: Mon, 19 Feb 2018 16:21:13 +0000 Subject: [PATCH] Adding parameter array support --- rclcpp/CMakeLists.txt | 10 + rclcpp/include/rclcpp/parameter.hpp | 191 +++++++++- rclcpp/src/rclcpp/parameter.cpp | 140 +++++-- rclcpp/test/test_parameter.cpp | 551 ++++++++++++++++++++++++++++ 4 files changed, 845 insertions(+), 47 deletions(-) create mode 100644 rclcpp/test/test_parameter.cpp diff --git a/rclcpp/CMakeLists.txt b/rclcpp/CMakeLists.txt index 7190709afc..bae66bba1e 100644 --- a/rclcpp/CMakeLists.txt +++ b/rclcpp/CMakeLists.txt @@ -190,6 +190,16 @@ if(BUILD_TESTING) target_link_libraries(test_node ${PROJECT_NAME}) endif() ament_add_gtest(test_parameter_events_filter test/test_parameter_events_filter.cpp) + ament_add_gtest(test_parameter test/test_parameter.cpp) + if(TARGET test_parameter) + target_include_directories(test_parameter PUBLIC + ${rcl_interfaces_INCLUDE_DIRS} + ${rmw_INCLUDE_DIRS} + ${rosidl_generator_cpp_INCLUDE_DIRS} + ${rosidl_typesupport_cpp_INCLUDE_DIRS} + ) + target_link_libraries(test_parameter ${PROJECT_NAME}) + endif() if(TARGET test_parameter_events_filter) target_include_directories(test_parameter_events_filter PUBLIC ${rcl_interfaces_INCLUDE_DIRS} diff --git a/rclcpp/include/rclcpp/parameter.hpp b/rclcpp/include/rclcpp/parameter.hpp index ce8de2e80f..533b57a958 100644 --- a/rclcpp/include/rclcpp/parameter.hpp +++ b/rclcpp/include/rclcpp/parameter.hpp @@ -15,6 +15,7 @@ #ifndef RCLCPP__PARAMETER_HPP_ #define RCLCPP__PARAMETER_HPP_ +#include #include #include #include @@ -39,6 +40,10 @@ enum ParameterType PARAMETER_DOUBLE = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE, PARAMETER_STRING = rcl_interfaces::msg::ParameterType::PARAMETER_STRING, PARAMETER_BYTE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY, + PARAMETER_BOOL_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY, + PARAMETER_INTEGER_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY, + PARAMETER_DOUBLE_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY, + PARAMETER_STRING_ARRAY = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY, }; // Structure to store an arbitrary parameter with templated get/set methods @@ -65,6 +70,30 @@ class ParameterVariant explicit ParameterVariant( const std::string & name, const std::vector & byte_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & bool_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & int_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & int_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & double_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & double_array_value); + RCLCPP_PUBLIC + explicit ParameterVariant( + const std::string & name, + const std::vector & string_array_value); RCLCPP_PUBLIC ParameterType @@ -84,6 +113,17 @@ class ParameterVariant // The following get_value() variants require the use of ParameterType + template + typename std::enable_if::type + get_value() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + // TODO(wjwwood): use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.bool_value; + } + template typename std::enable_if::type get_value() const @@ -118,30 +158,74 @@ class ParameterVariant } template - typename std::enable_if::type + typename std::enable_if< + type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type get_value() const { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL) { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { // TODO(wjwwood): use custom exception throw std::runtime_error("Invalid type"); } - return value_.bool_value; + return value_.byte_array_value; } template typename std::enable_if< - type == ParameterType::PARAMETER_BYTE_ARRAY, const std::vector &>::type + type == ParameterType::PARAMETER_BOOL_ARRAY, const std::vector &>::type get_value() const { - if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY) { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY) { // TODO(wjwwood): use custom exception throw std::runtime_error("Invalid type"); } - return value_.byte_array_value; + return value_.bool_array_value; + } + + template + typename std::enable_if< + type == ParameterType::PARAMETER_INTEGER_ARRAY, const std::vector &>::type + get_value() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY) { + // TODO(wjwwood): use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.integer_array_value; + } + + template + typename std::enable_if< + type == ParameterType::PARAMETER_DOUBLE_ARRAY, const std::vector &>::type + get_value() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY) { + // TODO(wjwwood): use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.double_array_value; + } + + template + typename std::enable_if< + type == ParameterType::PARAMETER_STRING_ARRAY, const std::vector &>::type + get_value() const + { + if (value_.type != rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY) { + // TODO(wjwwood): use custom exception + throw std::runtime_error("Invalid type"); + } + return value_.string_array_value; } // The following get_value() variants allow the use of primitive types + template + typename std::enable_if::value, bool>::type + get_value() const + { + return get_value(); + } + template typename std::enable_if< std::is_integral::value && !std::is_same::value, int64_t>::type @@ -165,21 +249,54 @@ class ParameterVariant } template - typename std::enable_if::value, bool>::type + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type get_value() const { - return get_value(); + return get_value(); } template typename std::enable_if< std::is_convertible< - type, const std::vector &>::value, const std::vector &>::type + type, const std::vector &>::value, const std::vector &>::type get_value() const { - return get_value(); + return get_value(); + } + + template + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get_value() const + { + return get_value(); } + template + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get_value() const + { + return get_value(); + } + + template + typename std::enable_if< + std::is_convertible< + type, const std::vector &>::value, const std::vector &>::type + get_value() const + { + return get_value(); + } + + RCLCPP_PUBLIC + bool + as_bool() const; + RCLCPP_PUBLIC int64_t as_int() const; @@ -193,12 +310,24 @@ class ParameterVariant as_string() const; RCLCPP_PUBLIC - bool - as_bool() const; + const std::vector & + as_byte_array() const; RCLCPP_PUBLIC - const std::vector & - as_bytes() const; + const std::vector & + as_bool_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_integer_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_double_array() const; + + RCLCPP_PUBLIC + const std::vector & + as_string_array() const; RCLCPP_PUBLIC static ParameterVariant @@ -208,16 +337,42 @@ class ParameterVariant rcl_interfaces::msg::Parameter to_parameter(); - RCLCPP_PUBLIC - std::string - value_to_string() const; + std::string value_to_string() const; private: + template + std::string + array_to_string( + const std::vector & array, + const std::ios::fmtflags format_flags = std::ios::dec) const + { + std::stringstream type_array; + bool first_item = true; + type_array << "["; + type_array.setf(format_flags, std::ios_base::basefield | std::ios::boolalpha); + type_array << std::showbase; + for (const ValType value : array) { + if (!first_item) { + type_array << ", "; + } else { + first_item = false; + } + type_array << static_cast(value); + } + type_array << "]"; + return type_array.str(); + } + + template + void vector_assign(OutputType & output, const InputType & input) + { + output.assign(input.begin(), input.end()); + } + std::string name_; rcl_interfaces::msg::ParameterValue value_; }; - /// Return a json encoded version of the parameter intended for a dict. RCLCPP_PUBLIC std::string diff --git a/rclcpp/src/rclcpp/parameter.cpp b/rclcpp/src/rclcpp/parameter.cpp index 7e0d289f74..5713b9ce0a 100644 --- a/rclcpp/src/rclcpp/parameter.cpp +++ b/rclcpp/src/rclcpp/parameter.cpp @@ -83,6 +83,54 @@ ParameterVariant::ParameterVariant( value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY; } +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & bool_array_value) +: name_(name) +{ + value_.bool_array_value = bool_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY; +} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & int_array_value) +: name_(name) +{ + vector_assign(value_.integer_array_value, int_array_value); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & int_array_value) +: name_(name) +{ + value_.integer_array_value = int_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY; +} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & double_array_value) +: name_(name) +{ + vector_assign(value_.double_array_value, double_array_value); + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & double_array_value) +: name_(name) +{ + value_.double_array_value = double_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY; +} + +ParameterVariant::ParameterVariant( + const std::string & name, const std::vector & string_array_value) +: name_(name) +{ + value_.string_array_value = string_array_value; + value_.type = rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY; +} + ParameterType ParameterVariant::get_type() const { @@ -93,6 +141,8 @@ std::string ParameterVariant::get_type_name() const { switch (get_type()) { + case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: + return "not set"; case rclcpp::parameter::ParameterType::PARAMETER_BOOL: return "bool"; case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: @@ -102,9 +152,15 @@ ParameterVariant::get_type_name() const case rclcpp::parameter::ParameterType::PARAMETER_STRING: return "string"; case rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY: - return "bytes"; - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; + return "byte_array"; + case rclcpp::parameter::ParameterType::PARAMETER_BOOL_ARRAY: + return "bool_array"; + case rclcpp::parameter::ParameterType::PARAMETER_INTEGER_ARRAY: + return "integer_array"; + case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE_ARRAY: + return "double_array"; + case rclcpp::parameter::ParameterType::PARAMETER_STRING_ARRAY: + return "string_array"; default: // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( @@ -125,6 +181,12 @@ ParameterVariant::get_parameter_value() const return value_; } +bool +ParameterVariant::as_bool() const +{ + return get_value(); +} + int64_t ParameterVariant::as_int() const { @@ -143,22 +205,42 @@ ParameterVariant::as_string() const return get_value(); } -bool -ParameterVariant::as_bool() const +const std::vector & +ParameterVariant::as_byte_array() const { - return get_value(); + return get_value(); } -const std::vector & -ParameterVariant::as_bytes() const +const std::vector & +ParameterVariant::as_bool_array() const { - return get_value(); + return get_value(); +} + +const std::vector & +ParameterVariant::as_integer_array() const +{ + return get_value(); +} + +const std::vector & +ParameterVariant::as_double_array() const +{ + return get_value(); +} + +const std::vector & +ParameterVariant::as_string_array() const +{ + return get_value(); } ParameterVariant ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & parameter) { switch (parameter.value.type) { + case PARAMETER_NOT_SET: + throw std::runtime_error("Type from ParameterValue is not set"); case PARAMETER_BOOL: return ParameterVariant(parameter.name, parameter.value.bool_value); case PARAMETER_INTEGER: @@ -169,8 +251,14 @@ ParameterVariant::from_parameter(const rcl_interfaces::msg::Parameter & paramete return ParameterVariant(parameter.name, parameter.value.string_value); case PARAMETER_BYTE_ARRAY: return ParameterVariant(parameter.name, parameter.value.byte_array_value); - case PARAMETER_NOT_SET: - throw std::runtime_error("Type from ParameterValue is not set"); + case PARAMETER_BOOL_ARRAY: + return ParameterVariant(parameter.name, parameter.value.bool_array_value); + case PARAMETER_INTEGER_ARRAY: + return ParameterVariant(parameter.name, parameter.value.integer_array_value); + case PARAMETER_DOUBLE_ARRAY: + return ParameterVariant(parameter.name, parameter.value.double_array_value); + case PARAMETER_STRING_ARRAY: + return ParameterVariant(parameter.name, parameter.value.string_array_value); default: // TODO(wjwwood): use custom exception // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) @@ -189,10 +277,11 @@ ParameterVariant::to_parameter() return parameter; } -std::string -ParameterVariant::value_to_string() const +std::string ParameterVariant::value_to_string() const { switch (get_type()) { + case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: + return "not set"; case rclcpp::parameter::ParameterType::PARAMETER_BOOL: return as_bool() ? "true" : "false"; case rclcpp::parameter::ParameterType::PARAMETER_INTEGER: @@ -202,22 +291,15 @@ ParameterVariant::value_to_string() const case rclcpp::parameter::ParameterType::PARAMETER_STRING: return as_string(); case rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY: - { - std::stringstream bytes; - bool first_byte = true; - bytes << "[" << std::hex; - for (auto & byte : as_bytes()) { - bytes << "0x" << byte; - if (!first_byte) { - bytes << ", "; - } else { - first_byte = false; - } - } - return bytes.str(); - } - case rclcpp::parameter::ParameterType::PARAMETER_NOT_SET: - return "not set"; + return array_to_string(as_byte_array(), std::ios::hex); + case rclcpp::parameter::ParameterType::PARAMETER_BOOL_ARRAY: + return array_to_string(as_bool_array(), std::ios::boolalpha); + case rclcpp::parameter::ParameterType::PARAMETER_INTEGER_ARRAY: + return array_to_string(as_integer_array()); + case rclcpp::parameter::ParameterType::PARAMETER_DOUBLE_ARRAY: + return array_to_string(as_double_array()); + case rclcpp::parameter::ParameterType::PARAMETER_STRING_ARRAY: + return array_to_string(as_string_array()); default: // *INDENT-OFF* (prevent uncrustify from making unnecessary indents here) throw std::runtime_error( diff --git a/rclcpp/test/test_parameter.cpp b/rclcpp/test/test_parameter.cpp new file mode 100644 index 0000000000..191ba46b51 --- /dev/null +++ b/rclcpp/test/test_parameter.cpp @@ -0,0 +1,551 @@ +// Copyright 2018 Open Source Robotics Foundation, Inc. +// +// 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 +#include +#include + +#include "rclcpp/parameter.hpp" +#include "rclcpp/rclcpp.hpp" + +class TestParameter : public ::testing::Test +{ +protected: + static void SetUpTestCase() + { + rclcpp::init(0, nullptr); + } +}; + +TEST(TestParameter, not_set_variant) { + // Direct instantiation + rclcpp::parameter::ParameterVariant not_set_variant; + EXPECT_EQ(not_set_variant.get_type(), rclcpp::parameter::PARAMETER_NOT_SET); + EXPECT_EQ(not_set_variant.get_type_name(), "not set"); + + EXPECT_THROW(not_set_variant.as_bool(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_int(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_double(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_string(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(not_set_variant.as_string_array(), std::runtime_error); + + rcl_interfaces::msg::Parameter not_set_param = not_set_variant.to_parameter(); + EXPECT_EQ(not_set_param.name, ""); + EXPECT_EQ(not_set_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_NOT_SET); + + // From parameter message + EXPECT_THROW(rclcpp::parameter::ParameterVariant::from_parameter(not_set_param), + std::runtime_error); +} + +TEST(TestParameter, bool_variant) { + // Direct instantiation + rclcpp::parameter::ParameterVariant bool_variant_true("bool_param", true); + EXPECT_EQ(bool_variant_true.get_name(), "bool_param"); + EXPECT_EQ(bool_variant_true.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(bool_variant_true.get_type_name(), "bool"); + EXPECT_TRUE(bool_variant_true.get_value()); + EXPECT_TRUE(bool_variant_true.get_parameter_value().bool_value); + EXPECT_EQ(bool_variant_true.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_TRUE(bool_variant_true.as_bool()); + + EXPECT_THROW(bool_variant_true.as_int(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_double(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_string(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_byte_array(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_bool_array(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_integer_array(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_double_array(), std::runtime_error); + EXPECT_THROW(bool_variant_true.as_string_array(), std::runtime_error); + + EXPECT_EQ(bool_variant_true.value_to_string(), "true"); + + rclcpp::parameter::ParameterVariant bool_variant_false("bool_param", false); + EXPECT_FALSE(bool_variant_false.get_value()); + EXPECT_FALSE(bool_variant_false.get_parameter_value().bool_value); + EXPECT_EQ(bool_variant_false.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + + rcl_interfaces::msg::Parameter bool_param = bool_variant_true.to_parameter(); + EXPECT_EQ(bool_param.name, "bool_param"); + EXPECT_EQ(bool_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(bool_param.value.bool_value, true); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg_true = + rclcpp::parameter::ParameterVariant::from_parameter(bool_param); + EXPECT_EQ(from_msg_true.get_name(), "bool_param"); + EXPECT_EQ(from_msg_true.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BOOL); + EXPECT_EQ(from_msg_true.get_type_name(), "bool"); + EXPECT_TRUE(from_msg_true.get_value()); + EXPECT_TRUE(from_msg_true.get_parameter_value().bool_value); + EXPECT_EQ(bool_variant_false.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); + + bool_param.value.bool_value = false; + rclcpp::parameter::ParameterVariant from_msg_false = + rclcpp::parameter::ParameterVariant::from_parameter(bool_param); + EXPECT_FALSE(from_msg_false.get_value()); + EXPECT_FALSE(from_msg_false.get_parameter_value().bool_value); + EXPECT_EQ(bool_variant_false.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL); +} + +TEST(TestParameter, integer_variant) { + const int TEST_VALUE {42}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant integer_variant("integer_param", TEST_VALUE); + EXPECT_EQ(integer_variant.get_name(), "integer_param"); + EXPECT_EQ(integer_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(integer_variant.get_type_name(), "integer"); + EXPECT_EQ(integer_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(integer_variant.get_parameter_value().integer_value, TEST_VALUE); + EXPECT_EQ(integer_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(integer_variant.as_int(), TEST_VALUE); + + EXPECT_THROW(integer_variant.as_bool(), std::runtime_error); + EXPECT_THROW(integer_variant.as_double(), std::runtime_error); + EXPECT_THROW(integer_variant.as_string(), std::runtime_error); + EXPECT_THROW(integer_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(integer_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(integer_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(integer_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(integer_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ(integer_variant.value_to_string(), "42"); + + const int64_t TEST_VALUE_L {std::numeric_limits::max()}; + + rclcpp::parameter::ParameterVariant long_variant("integer_param", TEST_VALUE_L); + EXPECT_EQ(long_variant.get_name(), "integer_param"); + EXPECT_EQ(long_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(long_variant.get_type_name(), "integer"); + EXPECT_EQ(long_variant.get_value(), + TEST_VALUE_L); + EXPECT_EQ(long_variant.get_parameter_value().integer_value, TEST_VALUE_L); + EXPECT_EQ(long_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + + rcl_interfaces::msg::Parameter integer_param = long_variant.to_parameter(); + EXPECT_EQ(integer_param.name, "integer_param"); + EXPECT_EQ(integer_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(integer_param.value.integer_value, TEST_VALUE_L); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(integer_param); + EXPECT_EQ(from_msg.get_name(), "integer_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_INTEGER); + EXPECT_EQ(from_msg.get_type_name(), "integer"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE_L); + EXPECT_EQ(from_msg.get_parameter_value().integer_value, TEST_VALUE_L); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER); +} + +TEST(TestParameter, double_variant) { + const double TEST_VALUE {-42.1}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant double_variant("double_param", TEST_VALUE); + EXPECT_EQ(double_variant.get_name(), "double_param"); + EXPECT_EQ(double_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(double_variant.get_type_name(), "double"); + EXPECT_EQ(double_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(double_variant.get_parameter_value().double_value, TEST_VALUE); + EXPECT_EQ(double_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(double_variant.as_double(), TEST_VALUE); + + EXPECT_THROW(double_variant.as_bool(), std::runtime_error); + EXPECT_THROW(double_variant.as_int(), std::runtime_error); + EXPECT_THROW(double_variant.as_string(), std::runtime_error); + EXPECT_THROW(double_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(double_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(double_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(double_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(double_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ(double_variant.value_to_string(), "-42.100000"); + + const float TEST_VALUE_F {static_cast(-TEST_VALUE)}; + rclcpp::parameter::ParameterVariant float_variant("float_param", TEST_VALUE_F); + EXPECT_EQ(float_variant.get_name(), "float_param"); + EXPECT_EQ(float_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(float_variant.get_type_name(), "double"); + EXPECT_EQ(float_variant.get_value(), + TEST_VALUE_F); + EXPECT_EQ(float_variant.get_parameter_value().double_value, TEST_VALUE_F); + EXPECT_EQ(float_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + + rcl_interfaces::msg::Parameter double_param = double_variant.to_parameter(); + EXPECT_EQ(double_param.name, "double_param"); + EXPECT_EQ(double_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(double_param.value.double_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(double_param); + EXPECT_EQ(from_msg.get_name(), "double_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_DOUBLE); + EXPECT_EQ(from_msg.get_type_name(), "double"); + EXPECT_EQ(from_msg.get_value(), TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().double_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE); +} + +TEST(TestParameter, string_variant) { + const std::string TEST_VALUE {"ROS2"}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant string_variant("string_param", TEST_VALUE); + EXPECT_EQ(string_variant.get_name(), "string_param"); + EXPECT_EQ(string_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_STRING); + EXPECT_EQ(string_variant.get_type_name(), "string"); + EXPECT_EQ(string_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(string_variant.get_parameter_value().string_value, TEST_VALUE); + EXPECT_EQ(string_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_EQ(string_variant.as_string(), TEST_VALUE); + + EXPECT_THROW(string_variant.as_bool(), std::runtime_error); + EXPECT_THROW(string_variant.as_int(), std::runtime_error); + EXPECT_THROW(string_variant.as_double(), std::runtime_error); + EXPECT_THROW(string_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(string_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(string_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(string_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(string_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ(string_variant.value_to_string(), TEST_VALUE); + + rcl_interfaces::msg::Parameter string_param = string_variant.to_parameter(); + EXPECT_EQ(string_param.name, "string_param"); + EXPECT_EQ(string_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_STRING); + EXPECT_EQ(string_param.value.string_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(string_param); + EXPECT_EQ(from_msg.get_name(), "string_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_STRING); + EXPECT_EQ(from_msg.get_type_name(), "string"); + EXPECT_EQ(from_msg.get_value(), TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().string_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_STRING); +} + +TEST(TestParameter, byte_array_variant) { + const std::vector TEST_VALUE {0x52, 0x4f, 0x53, 0x32}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant byte_array_variant("byte_array_param", TEST_VALUE); + EXPECT_EQ(byte_array_variant.get_name(), "byte_array_param"); + EXPECT_EQ(byte_array_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(byte_array_variant.get_type_name(), "byte_array"); + EXPECT_EQ(byte_array_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(byte_array_variant.get_parameter_value().byte_array_value, TEST_VALUE); + EXPECT_EQ(byte_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(byte_array_variant.as_byte_array(), TEST_VALUE); + + EXPECT_THROW(byte_array_variant.as_bool(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_int(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_double(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_string(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(byte_array_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ(byte_array_variant.value_to_string(), "[0x52, 0x4f, 0x53, 0x32]"); + + rcl_interfaces::msg::Parameter byte_array_param = byte_array_variant.to_parameter(); + EXPECT_EQ(byte_array_param.name, "byte_array_param"); + EXPECT_EQ(byte_array_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(byte_array_param.value.byte_array_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(byte_array_param); + EXPECT_EQ(from_msg.get_name(), "byte_array_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BYTE_ARRAY); + EXPECT_EQ(from_msg.get_type_name(), "byte_array"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().byte_array_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BYTE_ARRAY); +} + +TEST(TestParameter, bool_array_variant) { + const std::vector TEST_VALUE {false, true, true, false, false, true}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant bool_array_variant("bool_array_param", TEST_VALUE); + EXPECT_EQ(bool_array_variant.get_name(), "bool_array_param"); + EXPECT_EQ(bool_array_variant.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(bool_array_variant.get_type_name(), "bool_array"); + EXPECT_EQ(bool_array_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(bool_array_variant.get_parameter_value().bool_array_value, TEST_VALUE); + EXPECT_EQ(bool_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(bool_array_variant.as_bool_array(), TEST_VALUE); + + EXPECT_THROW(bool_array_variant.as_bool(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_int(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_double(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_string(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(bool_array_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ(bool_array_variant.value_to_string(), "[false, true, true, false, false, true]"); + + rcl_interfaces::msg::Parameter bool_array_param = bool_array_variant.to_parameter(); + EXPECT_EQ(bool_array_param.name, "bool_array_param"); + EXPECT_EQ(bool_array_param.value.type, rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(bool_array_param.value.bool_array_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(bool_array_param); + EXPECT_EQ(from_msg.get_name(), "bool_array_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_BOOL_ARRAY); + EXPECT_EQ(from_msg.get_type_name(), "bool_array"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().bool_array_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_BOOL_ARRAY); +} + +TEST(TestParameter, integer_array_variant) { + const std::vector TEST_VALUE + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant integer_array_variant("integer_array_param", TEST_VALUE); + EXPECT_EQ(integer_array_variant.get_name(), "integer_array_param"); + EXPECT_EQ(integer_array_variant.get_type(), + rclcpp::parameter::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(integer_array_variant.get_type_name(), "integer_array"); + + // No direct comparison of vectors of ints and long ints + const auto & param_value_ref = + integer_array_variant.get_value(); + const auto mismatches_get_val = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), + param_value_ref.begin()); + EXPECT_EQ(mismatches_get_val.first, TEST_VALUE.end()); + EXPECT_EQ(mismatches_get_val.second, param_value_ref.end()); + + const auto mismatches_get_param = std::mismatch(TEST_VALUE.begin(), TEST_VALUE.end(), + integer_array_variant.get_parameter_value().integer_array_value.begin()); + EXPECT_EQ(mismatches_get_param.first, TEST_VALUE.end()); + EXPECT_EQ(mismatches_get_param.second, + integer_array_variant.get_parameter_value().integer_array_value.end()); + + EXPECT_EQ(integer_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + + const std::vector TEST_VALUE_L + {42, -99, std::numeric_limits::max(), std::numeric_limits::lowest(), 0}; + + rclcpp::parameter::ParameterVariant long_array_variant("integer_array_param", TEST_VALUE_L); + EXPECT_EQ(long_array_variant.get_name(), "integer_array_param"); + EXPECT_EQ(long_array_variant.get_type(), + rclcpp::parameter::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(long_array_variant.get_type_name(), "integer_array"); + EXPECT_EQ( + long_array_variant.get_value(), + TEST_VALUE_L); + EXPECT_EQ(long_array_variant.get_parameter_value().integer_array_value, TEST_VALUE_L); + EXPECT_EQ(long_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(long_array_variant.as_integer_array(), TEST_VALUE_L); + + EXPECT_THROW(long_array_variant.as_bool(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_int(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_double(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_string(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_double_array(), std::runtime_error); + EXPECT_THROW(long_array_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ( + long_array_variant.value_to_string(), + "[42, -99, 9223372036854775807, -9223372036854775808, 0]"); + + rcl_interfaces::msg::Parameter integer_array_param = long_array_variant.to_parameter(); + EXPECT_EQ(integer_array_param.name, "integer_array_param"); + EXPECT_EQ(integer_array_param.value.type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(integer_array_param.value.integer_array_value, TEST_VALUE_L); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(integer_array_param); + EXPECT_EQ(from_msg.get_name(), "integer_array_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_INTEGER_ARRAY); + EXPECT_EQ(from_msg.get_type_name(), "integer_array"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE_L); + EXPECT_EQ(from_msg.get_parameter_value().integer_array_value, TEST_VALUE_L); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_INTEGER_ARRAY); +} + +TEST(TestParameter, double_array_variant) { + const std::vector TEST_VALUE + {42.1, -99.1, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant double_array_variant("double_array_param", TEST_VALUE); + EXPECT_EQ(double_array_variant.get_name(), "double_array_param"); + EXPECT_EQ(double_array_variant.get_type(), + rclcpp::parameter::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(double_array_variant.get_type_name(), "double_array"); + EXPECT_EQ( + double_array_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(double_array_variant.get_parameter_value().double_array_value, TEST_VALUE); + EXPECT_EQ(double_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(double_array_variant.as_double_array(), TEST_VALUE); + + EXPECT_THROW(double_array_variant.as_bool(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_int(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_double(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_string(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(double_array_variant.as_string_array(), std::runtime_error); + + EXPECT_EQ( + double_array_variant.value_to_string(), "[42.1, -99.1, 1.79769e+308, -1.79769e+308, 0.1]"); + + const std::vector TEST_VALUE_F + {42.1f, -99.1f, std::numeric_limits::max(), std::numeric_limits::lowest(), 0.1f}; + + rclcpp::parameter::ParameterVariant float_array_variant("double_array_param", TEST_VALUE_F); + EXPECT_EQ(float_array_variant.get_name(), "double_array_param"); + EXPECT_EQ(float_array_variant.get_type(), + rclcpp::parameter::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(float_array_variant.get_type_name(), "double_array"); + + // No direct comparison of vectors of doubles and floats + const auto & param_value_ref = + float_array_variant.get_value(); + const auto mismatches_get_val = std::mismatch(TEST_VALUE_F.begin(), TEST_VALUE_F.end(), + param_value_ref.begin()); + EXPECT_EQ(mismatches_get_val.first, TEST_VALUE_F.end()); + EXPECT_EQ(mismatches_get_val.second, param_value_ref.end()); + + const auto mismatches_get_param = std::mismatch(TEST_VALUE_F.begin(), TEST_VALUE_F.end(), + float_array_variant.get_parameter_value().double_array_value.begin()); + EXPECT_EQ(mismatches_get_param.first, TEST_VALUE_F.end()); + EXPECT_EQ(mismatches_get_param.second, + double_array_variant.get_parameter_value().double_array_value.end()); + + EXPECT_EQ(float_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + + rcl_interfaces::msg::Parameter double_array_param = double_array_variant.to_parameter(); + EXPECT_EQ(double_array_param.name, "double_array_param"); + EXPECT_EQ(double_array_param.value.type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(double_array_param.value.double_array_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(double_array_param); + EXPECT_EQ(from_msg.get_name(), "double_array_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_DOUBLE_ARRAY); + EXPECT_EQ(from_msg.get_type_name(), "double_array"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().double_array_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_DOUBLE_ARRAY); +} + +TEST(TestParameter, string_array_variant) { + const std::vector TEST_VALUE {"R", "O", "S2"}; + + // Direct instantiation + rclcpp::parameter::ParameterVariant string_array_variant("string_array_param", TEST_VALUE); + EXPECT_EQ(string_array_variant.get_name(), "string_array_param"); + EXPECT_EQ(string_array_variant.get_type(), + rclcpp::parameter::ParameterType::PARAMETER_STRING_ARRAY); + EXPECT_EQ(string_array_variant.get_type_name(), "string_array"); + EXPECT_EQ( + string_array_variant.get_value(), + TEST_VALUE); + EXPECT_EQ(string_array_variant.get_parameter_value().string_array_value, TEST_VALUE); + EXPECT_EQ(string_array_variant.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY); + EXPECT_EQ(string_array_variant.as_string_array(), TEST_VALUE); + + EXPECT_THROW(string_array_variant.as_bool(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_int(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_double(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_string(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_byte_array(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_bool_array(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_integer_array(), std::runtime_error); + EXPECT_THROW(string_array_variant.as_double_array(), std::runtime_error); + + EXPECT_EQ(string_array_variant.value_to_string(), "[R, O, S2]"); + + rcl_interfaces::msg::Parameter string_array_param = string_array_variant.to_parameter(); + EXPECT_EQ(string_array_param.name, "string_array_param"); + EXPECT_EQ(string_array_param.value.type, + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY); + EXPECT_EQ(string_array_param.value.string_array_value, TEST_VALUE); + + // From parameter message + rclcpp::parameter::ParameterVariant from_msg = + rclcpp::parameter::ParameterVariant::from_parameter(string_array_param); + EXPECT_EQ(from_msg.get_name(), "string_array_param"); + EXPECT_EQ(from_msg.get_type(), rclcpp::parameter::ParameterType::PARAMETER_STRING_ARRAY); + EXPECT_EQ(from_msg.get_type_name(), "string_array"); + EXPECT_EQ(from_msg.get_value(), + TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().string_array_value, TEST_VALUE); + EXPECT_EQ(from_msg.get_parameter_value().type, + rcl_interfaces::msg::ParameterType::PARAMETER_STRING_ARRAY); +}