From e18b93883962bc67a3d4c4e0ae5b777603fe84de Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Fri, 31 May 2024 16:06:13 +0200 Subject: [PATCH 1/4] fix: keep parameter values in sync with ros --- .../modulo_components/ComponentInterface.hpp | 25 +++++--- .../modulo_components/component_interface.py | 48 +++++++++++++-- .../src/ComponentInterface.cpp | 61 ++++++++++++------- ...t_component_interface_empty_parameters.cpp | 28 ++++++++- .../test_component_interface_parameters.cpp | 6 +- ...st_component_interface_empty_parameters.py | 27 +++++++- 6 files changed, 154 insertions(+), 41 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index d5b88c04..aea255bb 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -459,6 +459,13 @@ class ComponentInterface { /** * @brief Callback function to validate and update parameters on change. * @param parameters The new parameter objects provided by the ROS interface + */ + void pre_set_parameters_callback(std::vector& parameters); + + /** + * @brief Callback function notify ROS about the validation result from the pre_set_parameters_callback step.. + * @param parameters The new parameter objects provided by the ROS interface + * @see pre_set_parameters_callback() * @return The result of the validation */ rcl_interfaces::msg::SetParametersResult on_set_parameters_callback(const std::vector& parameters); @@ -574,8 +581,13 @@ class ComponentInterface { std::map> periodic_callbacks_; ///< Map of periodic function callbacks state_representation::ParameterMap parameter_map_; ///< ParameterMap for handling parameters + std::unordered_map read_only_parameters_; + std::shared_ptr + pre_set_parameter_cb_handle_; ///< ROS callback function handle on pre set of parameters std::shared_ptr - parameter_cb_handle_; ///< ROS callback function handle for setting parameters + on_set_parameter_cb_handle_; ///< ROS callback function handle on set of parameters + rcl_interfaces::msg::SetParametersResult set_parameters_result_; + bool pre_set_parameter_callback_called_ = false; ///< Flag to indicate if pre_set_parameter_callback was called std::shared_ptr step_timer_; ///< Timer for the step function std::shared_ptr tf_buffer_; ///< TF buffer @@ -591,8 +603,6 @@ class ComponentInterface { std::shared_ptr node_timers_; std::shared_ptr node_topics_; rclcpp::QoS qos_ = rclcpp::QoS(10); ///< Quality of Service for ROS publishers and subscribers - - bool set_parameter_callback_called_ = false; ///< Flag to indicate if on_set_parameter_callback was called }; template @@ -619,10 +629,11 @@ inline T ComponentInterface::get_parameter_value(const std::string& name) const template inline void ComponentInterface::set_parameter_value(const std::string& name, const T& value) { try { - rcl_interfaces::msg::SetParametersResult result = this->node_parameters_->set_parameters( - { - modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value)) - }).at(0); + std::vector parameters{ + modulo_core::translators::write_parameter(state_representation::make_shared_parameter(name, value))}; + this->pre_set_parameters_callback(parameters); + this->pre_set_parameter_callback_called_ = true; + auto result = this->node_parameters_->set_parameters(parameters).at(0); if (!result.successful) { RCLCPP_ERROR_STREAM_THROTTLE(this->node_logging_->get_logger(), *this->node_clock_->get_clock(), 1000, "Failed to set parameter value of parameter '" << name << "': " << result.reason); diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 9e318ed4..1b8dcd31 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -46,6 +46,9 @@ def __init__(self, node_name: str, *args, **kwargs): super().__init__(node_name, *args, **node_kwargs) self.__step_lock = Lock() self._parameter_dict: Dict[str, Union[str, sr.Parameter]] = {} + self.__read_only_parameters: Dict[str, bool] = {} + self.__pre_set_parameters_callback_called = False + self.__set_parameters_result = SetParametersResult() self._predicates: Dict[str, Union[bool, Callable[[], bool]]] = {} self._triggers: Dict[str, bool] = {} self._periodic_callbacks: Dict[str, Callable[[], None]] = {} @@ -60,6 +63,7 @@ def __init__(self, node_name: str, *args, **kwargs): self._qos = QoSProfile(depth=10) + self.add_pre_set_parameters_callback(self.__pre_set_parameters_callback) self.add_on_set_parameters_callback(self.__on_set_parameters_callback) self.add_parameter(sr.Parameter("rate", 10.0, sr.ParameterType.DOUBLE), "The rate in Hertz for all periodic callbacks") @@ -142,16 +146,25 @@ def add_parameter(self, parameter: Union[str, sr.Parameter], description: str, r if not self.has_parameter(sr_parameter.get_name()): self.get_logger().debug(f"Adding parameter '{sr_parameter.get_name()}'.") self._parameter_dict[sr_parameter.get_name()] = parameter + self.__read_only_parameters[sr_parameter.get_name()] = False try: descriptor = ParameterDescriptor(description=description, read_only=read_only) + self.__set_parameters_result = SetParametersResult(successful=True, reason="") if sr_parameter.is_empty(): descriptor.dynamic_typing = True descriptor.type = get_ros_parameter_type(sr_parameter.get_parameter_type()).value self.declare_parameter(ros_param.name, None, descriptor=descriptor) else: self.declare_parameter(ros_param.name, ros_param.value, descriptor=descriptor) + new_parameters = self.__pre_set_parameters_callback([Node.get_parameter(self, ros_param.name)]) + result = self.__on_set_parameters_callback(new_parameters) + if not result.successful: + self.undeclare_parameter(ros_param.name) + raise ParameterError(result.reason) + self.__read_only_parameters[sr_parameter.get_name()] = read_only except Exception as e: del self._parameter_dict[sr_parameter.get_name()] + del self.__read_only_parameters[sr_parameter.get_name()] raise ParameterError(f"Failed to add parameter: {e}") else: self.get_logger().debug(f"Parameter '{sr_parameter.get_name()}' already exists.") @@ -214,8 +227,10 @@ def set_parameter_value(self, name: str, value: T, parameter_type: sr.ParameterT :param parameter_type: The type of the parameter """ try: - ros_param = write_parameter(sr.Parameter(name, value, parameter_type)) - result = self.set_parameters([ros_param])[0] + parameters = [write_parameter(sr.Parameter(name, value, parameter_type))] + new_parameters = self.__pre_set_parameters_callback(parameters) + self.__pre_set_parameters_callback_called = True + result = self.set_parameters(new_parameters)[0] if not result.successful: self.get_logger().error(f"Failed to set parameter value of parameter '{name}': {result.reason}", throttle_duration_sec=1.0) @@ -249,20 +264,28 @@ def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: :return: The validation result """ return True - - def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult: + + def __pre_set_parameters_callback(self, ros_parameters: List[Parameter]) -> List[Parameter]: """ Callback function to validate and update parameters on change. :param ros_parameters: The new parameter objects provided by the ROS interface - :return: The result of the validation + :return: The validated parameter objects """ + if self.__pre_set_parameters_callback_called: + self.__pre_set_parameters_callback_called = False + return ros_parameters + new_parameters = [] result = SetParametersResult(successful=True) for ros_param in ros_parameters: try: parameter = self.__get_component_parameter(ros_param.name) + if self.__read_only_parameters.get(ros_param.name): + self.get_logger().debug(f"Parameter '{ros_param.name}' is read only") + continue new_parameter = read_parameter_const(ros_param, parameter) if not self.__validate_parameter(new_parameter): + new_parameters.append(write_parameter(parameter)) result.successful = False result.reason = f"Validation of parameter '{ros_param.name}' returned false!" else: @@ -270,9 +293,24 @@ def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetPa self.__setattr__(self._parameter_dict[ros_param.name], new_parameter) else: self._parameter_dict[ros_param.name] = new_parameter + new_parameters.append(write_parameter(new_parameter)) except Exception as e: result.successful = False result.reason += str(e) + self.__set_parameters_result = result + return new_parameters + + + def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult: + """ + Callback function notify ROS about the validation result from the pre_set_parameters_callback step + + :param ros_parameters: The parameter objects provided by the ROS interface + :return: The result of the validation + """ + result = copy.copy(self.__set_parameters_result) + self.__set_parameters_result.successful = True + self.__set_parameters_result.reason = "" return result def add_predicate(self, name: str, value: Union[bool, Callable[[], bool]]): diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index e72fe46d..0d22bd68 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -23,8 +23,10 @@ ComponentInterface::ComponentInterface( node_services_(interfaces->get_node_services_interface()), node_timers_(interfaces->get_node_timers_interface()), node_topics_(interfaces->get_node_topics_interface()) { - // register the parameter change callback handler - this->parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback( + // register the parameter change callback handlers + this->pre_set_parameter_cb_handle_ = this->node_parameters_->add_pre_set_parameters_callback( + [this](std::vector& parameters) { return this->pre_set_parameters_callback(parameters); }); + this->on_set_parameter_cb_handle_ = this->node_parameters_->add_on_set_parameters_callback( [this](const std::vector& parameters) -> rcl_interfaces::msg::SetParametersResult { return this->on_set_parameters_callback(parameters); }); @@ -79,9 +81,7 @@ void ComponentInterface::on_step_callback() {} void ComponentInterface::add_parameter( const std::shared_ptr& parameter, const std::string& description, - bool read_only -) { - this->set_parameter_callback_called_ = false; + bool read_only) { rclcpp::Parameter ros_param; try { ros_param = modulo_core::translators::write_parameter(parameter); @@ -91,10 +91,14 @@ void ComponentInterface::add_parameter( if (!this->node_parameters_->has_parameter(parameter->get_name())) { RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), "Adding parameter '" << parameter->get_name() << "'."); this->parameter_map_.set_parameter(parameter); + this->read_only_parameters_.insert_or_assign(parameter->get_name(), false); try { rcl_interfaces::msg::ParameterDescriptor descriptor; descriptor.description = description; descriptor.read_only = read_only; + // since the pre_set_parameters_callback is not called on parameter declaration, this has to be true + this->set_parameters_result_.successful = true; + this->set_parameters_result_.reason = ""; if (parameter->is_empty()) { descriptor.dynamic_typing = true; descriptor.type = modulo_core::translators::get_ros_parameter_type(parameter->get_parameter_type()); @@ -102,21 +106,22 @@ void ComponentInterface::add_parameter( } else { this->node_parameters_->declare_parameter(parameter->get_name(), ros_param.get_parameter_value(), descriptor); } - if (!this->set_parameter_callback_called_) { - auto result = - this->on_set_parameters_callback({this->node_parameters_->get_parameters({parameter->get_name()})}); - if (!result.successful) { - this->node_parameters_->undeclare_parameter(parameter->get_name()); - throw ParameterException(result.reason); - } + std::vector ros_parameters{this->node_parameters_->get_parameters({parameter->get_name()})}; + this->pre_set_parameters_callback(ros_parameters); + auto result = this->on_set_parameters_callback(ros_parameters); + if (!result.successful) { + this->node_parameters_->undeclare_parameter(parameter->get_name()); + throw ParameterException(result.reason); } + this->read_only_parameters_.at(parameter->get_name()) = read_only; } catch (const std::exception& ex) { this->parameter_map_.remove_parameter(parameter->get_name()); + this->read_only_parameters_.erase(parameter->get_name()); throw ParameterException("Failed to add parameter: " + std::string(ex.what())); } } else { - RCLCPP_DEBUG_STREAM(this->node_logging_->get_logger(), - "Parameter '" << parameter->get_name() << "' already exists."); + RCLCPP_DEBUG_STREAM( + this->node_logging_->get_logger(), "Parameter '" << parameter->get_name() << "' already exists."); } } @@ -129,33 +134,45 @@ ComponentInterface::get_parameter(const std::string& name) const { } } -rcl_interfaces::msg::SetParametersResult -ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { +void ComponentInterface::pre_set_parameters_callback(std::vector& parameters) { + if (this->pre_set_parameter_callback_called_) { + this->pre_set_parameter_callback_called_ = false; + return; + } rcl_interfaces::msg::SetParametersResult result; result.successful = true; - for (const auto& ros_parameter : parameters) { + for (auto& ros_parameter : parameters) { try { - if (ros_parameter.get_name().substr(0, 27) == "qos_overrides./tf.publisher") { + auto parameter = parameter_map_.get_parameter(ros_parameter.get_name()); + if (this->read_only_parameters_.at(ros_parameter.get_name())) { + RCLCPP_DEBUG_STREAM( + this->node_logging_->get_logger(), "Parameter '" << ros_parameter.get_name() << "' is read only."); continue; } - // get the associated parameter interface by name - auto parameter = parameter_map_.get_parameter(ros_parameter.get_name()); // convert the ROS parameter into a ParameterInterface without modifying the original auto new_parameter = modulo_core::translators::read_parameter_const(ros_parameter, parameter); if (!this->validate_parameter(new_parameter)) { + ros_parameter = modulo_core::translators::write_parameter(new_parameter); result.successful = false; result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!"; } else if (!new_parameter->is_empty()) { - // update the value of the parameter in the map modulo_core::translators::copy_parameter_value(new_parameter, parameter); + ros_parameter = modulo_core::translators::write_parameter(new_parameter); } } catch (const std::exception& ex) { result.successful = false; result.reason += ex.what(); } } - this->set_parameter_callback_called_ = true; + this->set_parameters_result_ = result; +} + +rcl_interfaces::msg::SetParametersResult +ComponentInterface::on_set_parameters_callback(const std::vector& parameters) { + auto result = this->set_parameters_result_; + this->set_parameters_result_.successful = true; + this->set_parameters_result_.reason = ""; return result; } diff --git a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp index f73f2428..0805d59b 100644 --- a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp @@ -13,11 +13,12 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface { ) : ComponentInterfacePublicInterface(node), allow_empty_(allow_empty) { if (add_parameter) { if (empty_parameter) { - this->add_parameter(std::make_shared>("name"), "Test parameter"); + this->add_parameter(std::make_shared>("name"), "Name parameter"); } else { - this->add_parameter(std::make_shared>("name", "test"), "Test parameter"); + this->add_parameter(std::make_shared>("name", "test"), "Name parameter"); } } + this->add_parameter(std::make_shared>("value", 0.0), "Value parameter"); }; private: @@ -30,6 +31,12 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface { RCLCPP_ERROR(this->node_logging_->get_logger(), "Provide a non empty value for parameter 'name'"); return false; } + } else if (parameter->get_name() == "value") { + auto value = parameter->get_parameter_value(); + if (value < 0) { + parameter->set_parameter_value(std::abs(value)); + } + return true; } return true; } @@ -180,4 +187,21 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ParameterOverridesEmpty) { EXPECT_THROW(std::make_shared>(node, false, true, false), modulo_utils::exceptions::ParameterException); } + +TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValueParameter) { + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 0); + this->component_->template set_parameter_value("value", 1); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 1); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 1); + this->component_->template set_parameter_value("value", -2); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 2); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 2); + + this->component_->set_ros_parameter({"value", 3.0}); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 3); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 3); + this->component_->set_ros_parameter({"value", -4.0}); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 4); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 4); +} } // namespace modulo_components diff --git a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp index 22ddc500..6b2e55da 100644 --- a/source/modulo_components/test/cpp/test_component_interface_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_parameters.cpp @@ -65,7 +65,7 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); this->component_->add_parameter(this->param_, "Test parameter"); - // Adding an existing parameter again should just set the value + // Adding an existing parameter again should not change the parameter this->component_->validate_parameter_was_called = false; EXPECT_NO_THROW(this->component_->add_parameter("test", 2, "foo")); EXPECT_FALSE(this->component_->validate_parameter_was_called); @@ -73,12 +73,12 @@ TYPED_TEST(ComponentInterfaceParameterTest, AddParameterAgain) { } TYPED_TEST(ComponentInterfaceParameterTest, SetParameter) { - // setting before adding should not work and parameter should not be created + // Setting before adding should not work and parameter should not be created this->component_->set_parameter_value("test", 1); EXPECT_THROW( this->component_->template get_parameter_value("test"), modulo_utils::exceptions::ParameterException); - // validation should not be invoked as the parameter did not exist + // Validation should not be invoked as the parameter did not exist EXPECT_FALSE(this->component_->validate_parameter_was_called); EXPECT_THROW(this->component_->get_ros_parameter("test"), rclcpp::exceptions::ParameterNotDeclaredException); diff --git a/source/modulo_components/test/python/test_component_interface_empty_parameters.py b/source/modulo_components/test/python/test_component_interface_empty_parameters.py index 0a9e7c56..1fd1f0f6 100644 --- a/source/modulo_components/test/python/test_component_interface_empty_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_empty_parameters.py @@ -15,9 +15,10 @@ def __init__(self, node_name, allow_empty=True, add_parameter=True, empty_parame self._allow_empty = allow_empty if add_parameter: if empty_parameter: - self.add_parameter(sr.Parameter("name", sr.ParameterType.STRING), "Test parameter") + self.add_parameter(sr.Parameter("name", sr.ParameterType.STRING), "Name parameter") else: - self.add_parameter(sr.Parameter("name", "test", sr.ParameterType.STRING), "Test parameter") + self.add_parameter(sr.Parameter("name", "test", sr.ParameterType.STRING), "Name parameter") + self.add_parameter(sr.Parameter("value", 0.0, sr.ParameterType.DOUBLE), "Value parameter") def get_ros_parameter(self, name: str) -> rclpy.Parameter: return rclpy.node.Node.get_parameter(self, name) @@ -32,6 +33,11 @@ def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: elif not parameter.get_value(): self.get_logger().error("Provide a non empty value for parameter 'name'") return False + elif parameter.get_name() == "value": + value = parameter.get_value() + if value < 0.0: + parameter.set_value(abs(value)) + return True return True @@ -150,3 +156,20 @@ def test_parameter_overrides_empty(ros_context): with pytest.raises(ParameterError): EmtpyParameterInterface("component", allow_empty=False, empty_parameter=False, parameter_overrides=[Parameter("name")]) + + +def test_value_parameter(component_test): + assert component_test.get_parameter_value("value") == 0.0 + component_test.set_parameter_value("value", 1.0, sr.ParameterType.DOUBLE) + assert component_test.get_parameter_value("value") == 1.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 1.0 + component_test.set_parameter_value("value", -2.0, sr.ParameterType.DOUBLE) + assert component_test.get_parameter_value("value") == 2.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 2.0 + + component_test.set_ros_parameter(Parameter("value", value=3.0)) + assert component_test.get_parameter_value("value") == 3.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 3.0 + component_test.set_ros_parameter(Parameter("value", value=-4.0)) + assert component_test.get_parameter_value("value") == 4.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 4.0 From 11126802bb3a15e0c79ed05a0c76f647738e0856 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 10 Jun 2024 09:56:22 +0000 Subject: [PATCH 2/4] fix: code review --- .../include/modulo_components/ComponentInterface.hpp | 2 +- .../modulo_components/component_interface.py | 4 ++-- source/modulo_components/src/ComponentInterface.cpp | 4 ++-- .../cpp/test_component_interface_empty_parameters.cpp | 9 +++++++++ .../test_component_interface_empty_parameters.py | 10 +++++++++- 5 files changed, 23 insertions(+), 6 deletions(-) diff --git a/source/modulo_components/include/modulo_components/ComponentInterface.hpp b/source/modulo_components/include/modulo_components/ComponentInterface.hpp index aea255bb..7a144488 100644 --- a/source/modulo_components/include/modulo_components/ComponentInterface.hpp +++ b/source/modulo_components/include/modulo_components/ComponentInterface.hpp @@ -463,7 +463,7 @@ class ComponentInterface { void pre_set_parameters_callback(std::vector& parameters); /** - * @brief Callback function notify ROS about the validation result from the pre_set_parameters_callback step.. + * @brief Callback function to notify ROS about the validation result from the pre_set_parameters_callback step. * @param parameters The new parameter objects provided by the ROS interface * @see pre_set_parameters_callback() * @return The result of the validation diff --git a/source/modulo_components/modulo_components/component_interface.py b/source/modulo_components/modulo_components/component_interface.py index 1b8dcd31..06c994a4 100644 --- a/source/modulo_components/modulo_components/component_interface.py +++ b/source/modulo_components/modulo_components/component_interface.py @@ -285,7 +285,7 @@ def __pre_set_parameters_callback(self, ros_parameters: List[Parameter]) -> List continue new_parameter = read_parameter_const(ros_param, parameter) if not self.__validate_parameter(new_parameter): - new_parameters.append(write_parameter(parameter)) + new_parameters.append(ros_param) result.successful = False result.reason = f"Validation of parameter '{ros_param.name}' returned false!" else: @@ -303,7 +303,7 @@ def __pre_set_parameters_callback(self, ros_parameters: List[Parameter]) -> List def __on_set_parameters_callback(self, ros_parameters: List[Parameter]) -> SetParametersResult: """ - Callback function notify ROS about the validation result from the pre_set_parameters_callback step + Callback function to notify ROS about the validation result from the pre_set_parameters_callback step :param ros_parameters: The parameter objects provided by the ROS interface :return: The result of the validation diff --git a/source/modulo_components/src/ComponentInterface.cpp b/source/modulo_components/src/ComponentInterface.cpp index 0d22bd68..dd98d9fb 100644 --- a/source/modulo_components/src/ComponentInterface.cpp +++ b/source/modulo_components/src/ComponentInterface.cpp @@ -153,10 +153,10 @@ void ComponentInterface::pre_set_parameters_callback(std::vectorvalidate_parameter(new_parameter)) { - ros_parameter = modulo_core::translators::write_parameter(new_parameter); result.successful = false; result.reason += "Validation of parameter '" + ros_parameter.get_name() + "' returned false!"; } else if (!new_parameter->is_empty()) { + // update the value of the parameter in the map modulo_core::translators::copy_parameter_value(new_parameter, parameter); ros_parameter = modulo_core::translators::write_parameter(new_parameter); } @@ -169,7 +169,7 @@ void ComponentInterface::pre_set_parameters_callback(std::vector& parameters) { +ComponentInterface::on_set_parameters_callback(const std::vector&) { auto result = this->set_parameters_result_; this->set_parameters_result_.successful = true; this->set_parameters_result_.reason = ""; diff --git a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp index 0805d59b..2afb046e 100644 --- a/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp +++ b/source/modulo_components/test/cpp/test_component_interface_empty_parameters.cpp @@ -36,6 +36,9 @@ class EmptyParameterInterface : public ComponentInterfacePublicInterface { if (value < 0) { parameter->set_parameter_value(std::abs(value)); } + if (std::abs(value) > 10) { + return false; + } return true; } return true; @@ -196,6 +199,9 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValueParameter) { this->component_->template set_parameter_value("value", -2); EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 2); EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 2); + this->component_->template set_parameter_value("value", 11); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 2); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 2); this->component_->set_ros_parameter({"value", 3.0}); EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 3); @@ -203,5 +209,8 @@ TYPED_TEST(ComponentInterfaceEmptyParameterTest, ValueParameter) { this->component_->set_ros_parameter({"value", -4.0}); EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 4); EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 4); + this->component_->set_ros_parameter({"value", -11.0}); + EXPECT_EQ(this->component_->get_parameter("value")->template get_parameter_value(), 4); + EXPECT_EQ(this->component_->get_ros_parameter("value").as_double(), 4); } } // namespace modulo_components diff --git a/source/modulo_components/test/python/test_component_interface_empty_parameters.py b/source/modulo_components/test/python/test_component_interface_empty_parameters.py index 1fd1f0f6..264a6a05 100644 --- a/source/modulo_components/test/python/test_component_interface_empty_parameters.py +++ b/source/modulo_components/test/python/test_component_interface_empty_parameters.py @@ -37,7 +37,9 @@ def on_validate_parameter_callback(self, parameter: sr.Parameter) -> bool: value = parameter.get_value() if value < 0.0: parameter.set_value(abs(value)) - return True + if abs(value) > 10: + return False + return True return True @@ -166,6 +168,9 @@ def test_value_parameter(component_test): component_test.set_parameter_value("value", -2.0, sr.ParameterType.DOUBLE) assert component_test.get_parameter_value("value") == 2.0 assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 2.0 + component_test.set_parameter_value("value", 11.0, sr.ParameterType.DOUBLE) + assert component_test.get_parameter_value("value") == 2.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 2.0 component_test.set_ros_parameter(Parameter("value", value=3.0)) assert component_test.get_parameter_value("value") == 3.0 @@ -173,3 +178,6 @@ def test_value_parameter(component_test): component_test.set_ros_parameter(Parameter("value", value=-4.0)) assert component_test.get_parameter_value("value") == 4.0 assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 4.0 + component_test.set_ros_parameter(Parameter("value", value=-11.0)) + assert component_test.get_parameter_value("value") == 4.0 + assert component_test.get_ros_parameter("value").get_parameter_value().double_value == 4.0 From b0e6e37a294ca4761a0969f1c44d03a1ebcfcd82 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Mon, 10 Jun 2024 09:59:06 +0000 Subject: [PATCH 3/4] docs: changelog --- CHANGELOG.md | 1 + 1 file changed, 1 insertion(+) diff --git a/CHANGELOG.md b/CHANGELOG.md index a4fbe81d..482e5929 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -28,6 +28,7 @@ Release Versions: - refactor: remove period parameter and make rate double (#108) - feat(modulo-controllers): use exceptions from modulo utils and update to CL v8.1.0 (#106) - feat: define topic validation warning in utilities (#101) +- fix: keep parameter values in sync with ros (#111) ## 4.2.1 From ab1db2c4ee6fbbc754fcfa612e51e666061003e5 Mon Sep 17 00:00:00 2001 From: Dominic Reber Date: Tue, 11 Jun 2024 07:24:15 +0200 Subject: [PATCH 4/4] docs: update changelog --- CHANGELOG.md | 16 ++++++++-------- 1 file changed, 8 insertions(+), 8 deletions(-) diff --git a/CHANGELOG.md b/CHANGELOG.md index 482e5929..62581ae2 100644 --- a/CHANGELOG.md +++ b/CHANGELOG.md @@ -16,19 +16,19 @@ Release Versions: ## Upcoming changes -- fix(modulo-controllers): improve log message (#110) -- fix(modulo-components): add missing space in log message (#113) -- feat(modulo-controller): add missing docs (#112) +- fix(controllers): improve log message (#110) +- fix(components): add missing space in log message (#113) +- feat(controllers): add missing docs (#112) - refactor: rename function in modulo translators (#77) -- feat: add mutex around step callback (#67) +- feat(components): add mutex around step callback (#67) - refactor: move utilities to modulo_utils package (#89) - refactor: move exceptions to modulo_utils (#90) - refactor: remove modulo_component_interfaces (#88) -- feat: add add_interfaces function for derived controllers (#102) -- refactor: remove period parameter and make rate double (#108) -- feat(modulo-controllers): use exceptions from modulo utils and update to CL v8.1.0 (#106) +- feat(controllers): add add_interfaces function for derived controllers (#102) +- refactor(components): remove period parameter and make rate double (#108) +- feat(controllers): use exceptions from modulo utils and update to CL v8.1.0 (#106) - feat: define topic validation warning in utilities (#101) -- fix: keep parameter values in sync with ros (#111) +- fix(components): keep parameter values in sync with ros (#111) ## 4.2.1