diff --git a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp index fbb6760268..5a6e036ac8 100644 --- a/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp +++ b/nav2_mppi_controller/include/nav2_mppi_controller/tools/parameters_handler.hpp @@ -100,9 +100,11 @@ class ParametersHandler * @brief Set a parameter to a dynamic parameter callback * @param setting Parameter * @param name Name of parameter + * @param param_type Type of parameter (dynamic or static) */ template - void setDynamicParamCallback(T & setting, const std::string & name); + void setDynamicParamCallback( + T & setting, const std::string & name, ParameterType param_type = ParameterType::Dynamic); /** * @brief Get mutex lock for changing parameters @@ -208,10 +210,7 @@ void ParametersHandler::getParam( node, name, rclcpp::ParameterValue(default_value)); setParam(setting, name, node); - - if (param_type == ParameterType::Dynamic) { - setDynamicParamCallback(setting, name); - } + setDynamicParamCallback(setting, name, param_type); } template @@ -224,13 +223,14 @@ void ParametersHandler::setParam( } template -void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name) +void ParametersHandler::setDynamicParamCallback( + T & setting, const std::string & name, ParameterType param_type) { if (get_param_callbacks_.find(name) != get_param_callbacks_.end()) { return; } - auto callback = [this, &setting, name](const rclcpp::Parameter & param) { + auto dynamic_callback = [this, &setting, name](const rclcpp::Parameter & param) { setting = as(param); if (verbose_) { @@ -238,10 +238,17 @@ void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & } }; - addDynamicParamCallback(name, callback); + auto static_callback = [this, &setting, name](const rclcpp::Parameter & param) { + if (verbose_) { + RCLCPP_DEBUG(logger_, "Ignoring change to static parameter: %s", + std::to_string(param).c_str()); + } + }; - if (verbose_) { - RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str()); + if (param_type == ParameterType::Dynamic) { + addDynamicParamCallback(name, dynamic_callback); + } else { + addDynamicParamCallback(name, static_callback); } } diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index c66d572561..ac5c392578 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -38,13 +38,14 @@ ParametersHandler::~ParametersHandler() void ParametersHandler::start() { auto node = node_.lock(); + + auto get_param = getParamGetter(node_name_); + get_param(verbose_, "verbose", false); + on_set_param_handler_ = node->add_on_set_parameters_callback( std::bind( &ParametersHandler::dynamicParamsCallback, this, std::placeholders::_1)); - - auto get_param = getParamGetter(node_name_); - get_param(verbose_, "verbose", false); } rcl_interfaces::msg::SetParametersResult @@ -53,6 +54,7 @@ ParametersHandler::dynamicParamsCallback( { rcl_interfaces::msg::SetParametersResult result; std::lock_guard lock(parameters_change_mutex_); + bool success = true; for (auto & pre_cb : pre_callbacks_) { pre_cb(); @@ -66,7 +68,8 @@ ParametersHandler::dynamicParamsCallback( { callback->second(param); } else { - RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str()); + RCLCPP_ERROR(logger_, "Parameter callback func for '%s' not found", param_name.c_str()); + success = false; } } @@ -74,7 +77,7 @@ ParametersHandler::dynamicParamsCallback( post_cb(); } - result.successful = true; + result.successful = success; return result; } diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index bcc3208e78..30b22bc3e5 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -168,8 +168,49 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) node->get_node_services_interface()); auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10)}); + { + rclcpp::Parameter("my_node.verbose", true), + rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10) + }); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + // Now, only param1 should change, param 2 should be the same + EXPECT_EQ(p1, 10); + EXPECT_EQ(p2, 7); +} + +TEST(ParameterHandlerTest, DynamicAndStaticParametersNotVerboseTest) +{ + auto node = std::make_shared("my_node"); + node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("static_int", rclcpp::ParameterValue(7)); + ParametersHandlerWrapper handler(node); + handler.start(); + + // Get parameters and check they have initial values + auto getParamer = handler.getParamGetter(""); + int p1 = 0, p2 = 0; + getParamer(p1, "dynamic_int", 0, ParameterType::Dynamic); + getParamer(p2, "static_int", 0, ParameterType::Static); + EXPECT_EQ(p1, 7); + EXPECT_EQ(p2, 7); + + // Now change them both via dynamic parameters + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + { + // Don't set default param rclcpp::Parameter("my_node.verbose", false), + rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10) + }); rclcpp::spin_until_future_complete( node->get_node_base_interface(), @@ -179,3 +220,36 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) EXPECT_EQ(p1, 10); EXPECT_EQ(p2, 7); } + +TEST(ParameterHandlerTest, DynamicAndStaticParametersNotDeclaredTest) +{ + auto node = std::make_shared("my_node"); + + node->declare_parameter("dynamic_int", rclcpp::ParameterValue(7)); + node->declare_parameter("static_int", rclcpp::ParameterValue(7)); + ParametersHandlerWrapper handler(node); + handler.start(); + + // Set verbose true to get more information about bad parameter usage + auto getParamer = handler.getParamGetter(""); + auto rec_param = std::make_shared( + node->get_node_base_interface(), node->get_node_topics_interface(), + node->get_node_graph_interface(), + node->get_node_services_interface()); + + auto results = rec_param->set_parameters_atomically( + { + rclcpp::Parameter("my_node.verbose", true), + }); + + rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + results); + + // Try to access some parameters that have not been declared + int p1 = 0, p2 = 0; + EXPECT_THROW(getParamer(p1, "not_declared", 8, ParameterType::Dynamic), + rclcpp::exceptions::InvalidParameterValueException); + EXPECT_THROW(getParamer(p2, "not_declared2", 9, ParameterType::Static), + rclcpp::exceptions::InvalidParameterValueException); +}