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 fbb6760268f..e96bca15984 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 @@ -210,7 +210,16 @@ void ParametersHandler::getParam( setParam(setting, name, node); if (param_type == ParameterType::Dynamic) { + if (verbose_) { + RCLCPP_INFO(node->get_logger(), "setDynamicParamCallback for %s", name.c_str()); + } setDynamicParamCallback(setting, name); + } else { + if (verbose_) { + RCLCPP_DEBUG( + node->get_logger(), "ParameterType::Static therefore no setDynamicParamCallback for %s", + name.c_str()); + } } } diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index c66d5725615..b144bf93bd3 100644 --- a/nav2_mppi_controller/src/parameters_handler.cpp +++ b/nav2_mppi_controller/src/parameters_handler.cpp @@ -38,13 +38,15 @@ ParametersHandler::~ParametersHandler() void ParametersHandler::start() { auto node = node_.lock(); + + // Register the special case "verbose" parameter before registering dynamicParamsCallback + 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 +55,7 @@ ParametersHandler::dynamicParamsCallback( { rcl_interfaces::msg::SetParametersResult result; std::lock_guard lock(parameters_change_mutex_); + bool success; for (auto & pre_cb : pre_callbacks_) { pre_cb(); @@ -66,7 +69,13 @@ ParametersHandler::dynamicParamsCallback( { callback->second(param); } else { - RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str()); + if (verbose_) { + // Expected if static parameter, ie one with no registered callback is attempted + // to be changed + RCLCPP_WARN(logger_, "Parameter callback func for '%s' not found", param_name.c_str()); + } + // success = true; // Decision ??? Still return true to avoid exception ??? + success = false; } } @@ -74,7 +83,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 bcc3208e784..c2a628fe163 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(),