From 2900652f91ab8ad14fe077a1e17e18fd4a639f26 Mon Sep 17 00:00:00 2001 From: aosmw <116058035+aosmw@users.noreply.github.com> Date: Fri, 22 Nov 2024 02:27:08 +1100 Subject: [PATCH] mppi parameters_handler: Improve verbose handling (#4704) (#4711) * mppi parameters_handler: Improve verbose handling (#4704) The "verbose" parameter of the parameters_handler is a special case that needs registration before the dynamic parameter handler callback is registered. In verbose mode make the parameter handler info/warn/debug messages more expressive. Signed-off-by: Mike Wake * Address review comments. (#4704) * remove comments. * Use RCLCPP_DEBUG instead of INFO for low level messages. * Add test for trying to access parameters that are not declared. Signed-off-by: Mike Wake * mppi parameters_handler: Improve static/dynamic/not defined logging (#4704) Attempts to change undefined parameters will not be successful and will log an error. Attempts to change static parameters will be ignored, a debug message is logged if a change in parameters is attempted. Signed-off-by: Mike Wake * mppi parameters_handler: populate SetParametersResult (#4704) Provide a mechanism to populate an rcl_interfaces::msg::SetParametersResult with the reasons for unsuccessful parameter setting, so that it may be propogated back to a set parameter client. The mechanism provides feedback when attempting to set undefined parameters, static parameters and could be used to validate dynamic parameters and provide a reason for rejection. NOTE: This changes public interface of ParametersHandler class. s/setDynamicParamCallback/setParamCallback s/addDynamicParamCallback/addParamCallback which takes a callback function that is able to populate a rcl_interfaces::msg::SetParametersResult. In order to indicate an unsuccessful parameter change and the reason, callback functions should append a "\n" to the reason before appending to if it is not empty. Signed-off-by: Mike Wake * mppi parameters_handler: fix reason handling and improve tests Signed-off-by: Mike Wake --------- Signed-off-by: Mike Wake Signed-off-by: Jakubach --- .../tools/parameters_handler.hpp | 56 +++++--- .../src/critics/cost_critic.cpp | 5 +- .../src/parameters_handler.cpp | 22 ++- .../test/parameter_handler_test.cpp | 133 ++++++++++++++++-- 4 files changed, 182 insertions(+), 34 deletions(-) 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..43bcc0b540c 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 @@ -36,12 +36,13 @@ enum class ParameterType { Dynamic, Static }; /** * @class mppi::ParametersHandler - * @brief Handles getting parameters and dynamic parmaeter changes + * @brief Handles getting parameters and dynamic parameter changes */ class ParametersHandler { public: - using get_param_func_t = void (const rclcpp::Parameter & param); + using get_param_func_t = void (const rclcpp::Parameter & param, + rcl_interfaces::msg::SetParametersResult & result); using post_callback_t = void (); using pre_callback_t = void (); @@ -100,9 +101,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 setParamCallback( + T & setting, const std::string & name, ParameterType param_type = ParameterType::Dynamic); /** * @brief Get mutex lock for changing parameters @@ -114,12 +117,20 @@ class ParametersHandler } /** - * @brief Set a parameter to a dynamic parameter callback + * @brief register a function to be called when setting a parameter + * + * The callback funciton is expected to behave as follows. + * Successful parameter changes should not interfere with + * the result parameter. + * Unsuccessful parameter changes should set the result.successful = false + * The result.reason should have "\n" appended if not empty before + * appending the reason that setting THIS parameter has failed. + * * @param name Name of parameter * @param callback Parameter callback */ template - void addDynamicParamCallback(const std::string & name, T && callback); + void addParamCallback(const std::string & name, T && callback); protected: /** @@ -160,8 +171,7 @@ class ParametersHandler bool verbose_{false}; - std::unordered_map> - get_param_callbacks_; + std::unordered_map> get_param_callbacks_; std::vector> pre_callbacks_; std::vector> post_callbacks_; @@ -179,7 +189,7 @@ inline auto ParametersHandler::getParamGetter(const std::string & ns) } template -void ParametersHandler::addDynamicParamCallback(const std::string & name, T && callback) +void ParametersHandler::addParamCallback(const std::string & name, T && callback) { get_param_callbacks_[name] = callback; } @@ -208,10 +218,7 @@ void ParametersHandler::getParam( node, name, rclcpp::ParameterValue(default_value)); setParam(setting, name, node); - - if (param_type == ParameterType::Dynamic) { - setDynamicParamCallback(setting, name); - } + setParamCallback(setting, name, param_type); } template @@ -224,24 +231,37 @@ void ParametersHandler::setParam( } template -void ParametersHandler::setDynamicParamCallback(T & setting, const std::string & name) +void ParametersHandler::setParamCallback( + 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, rcl_interfaces::msg::SetParametersResult & /*result*/) { setting = as(param); - if (verbose_) { RCLCPP_INFO(logger_, "Dynamic parameter changed: %s", std::to_string(param).c_str()); } }; - addDynamicParamCallback(name, callback); + auto static_callback = + [this, &setting, name]( + const rclcpp::Parameter & param, rcl_interfaces::msg::SetParametersResult & result) { + std::string reason = "Rejected change to static parameter: " + std::to_string(param); + result.successful = false; + if (!result.reason.empty()) { + result.reason += "\n"; + } + result.reason += reason; + }; - if (verbose_) { - RCLCPP_INFO(logger_, "Dynamic Parameter added %s", name.c_str()); + if (param_type == ParameterType::Dynamic) { + addParamCallback(name, dynamic_callback); + } else { + addParamCallback(name, static_callback); } } diff --git a/nav2_mppi_controller/src/critics/cost_critic.cpp b/nav2_mppi_controller/src/critics/cost_critic.cpp index f348d7780a1..e75c68f6c29 100644 --- a/nav2_mppi_controller/src/critics/cost_critic.cpp +++ b/nav2_mppi_controller/src/critics/cost_critic.cpp @@ -35,10 +35,11 @@ void CostCritic::initialize() weight_ /= 254.0f; // Normalize weight when parameter is changed dynamically as well - auto weightDynamicCb = [&](const rclcpp::Parameter & weight) { + auto weightDynamicCb = [&]( + const rclcpp::Parameter & weight, rcl_interfaces::msg::SetParametersResult & /*result*/) { weight_ = weight.as_double() / 254.0f; }; - parameters_handler_->addDynamicParamCallback(name_ + ".cost_weight", weightDynamicCb); + parameters_handler_->addParamCallback(name_ + ".cost_weight", weightDynamicCb); collision_checker_.setCostmap(costmap_); possible_collision_cost_ = findCircumscribedCost(costmap_ros_); diff --git a/nav2_mppi_controller/src/parameters_handler.cpp b/nav2_mppi_controller/src/parameters_handler.cpp index c66d5725615..4dd20556303 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 @@ -52,6 +53,9 @@ ParametersHandler::dynamicParamsCallback( std::vector parameters) { rcl_interfaces::msg::SetParametersResult result; + result.successful = true; + result.reason = ""; + std::lock_guard lock(parameters_change_mutex_); for (auto & pre_cb : pre_callbacks_) { @@ -64,9 +68,13 @@ ParametersHandler::dynamicParamsCallback( if (auto callback = get_param_callbacks_.find(param_name); callback != get_param_callbacks_.end()) { - callback->second(param); + callback->second(param, result); } else { - RCLCPP_WARN(logger_, "Parameter %s not found", param_name.c_str()); + result.successful = false; + if (!result.reason.empty()) { + result.reason += "\n"; + } + result.reason += "get_param_callback func for '" + param_name + "' not found.\n"; } } @@ -74,7 +82,9 @@ ParametersHandler::dynamicParamsCallback( post_cb(); } - result.successful = true; + if (!result.successful) { + RCLCPP_ERROR(logger_, result.reason.c_str()); + } return result; } diff --git a/nav2_mppi_controller/test/parameter_handler_test.cpp b/nav2_mppi_controller/test/parameter_handler_test.cpp index bcc3208e784..27ef61d90c7 100644 --- a/nav2_mppi_controller/test/parameter_handler_test.cpp +++ b/nav2_mppi_controller/test/parameter_handler_test.cpp @@ -93,7 +93,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) post_triggered = true; }; - auto dynamicCb = [&](const rclcpp::Parameter & /*param*/) { + auto dynamicCb = [&](const rclcpp::Parameter & /*param*/, + rcl_interfaces::msg::SetParametersResult & /*result*/) { dynamic_triggered = true; }; @@ -104,8 +105,8 @@ TEST(ParameterHandlerTest, PrePostDynamicCallbackTest) ParametersHandlerWrapper a; a.addPreCallback(preCb); a.addPostCallback(postCb); - a.addDynamicParamCallback("use_sim_time", dynamicCb); - a.setDynamicParamCallback(val, "blah_blah"); + a.addParamCallback("use_sim_time", dynamicCb); + a.setParamCallback(val, "blah_blah"); // Dynamic callback should not trigger, wrong parameter, but val should be updated a.dynamicParamsCallback(std::vector{random_param}); @@ -146,6 +147,52 @@ TEST(ParameterHandlerTest, GetSystemParamsTest) } TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) +{ + 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()); + + std::shared_future result_future = + rec_param->set_parameters_atomically({ + rclcpp::Parameter("my_node.verbose", true), + rclcpp::Parameter("dynamic_int", 10), + rclcpp::Parameter("static_int", 10) + }); + + auto rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") + + "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); + + // 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)); @@ -167,15 +214,85 @@ TEST(ParameterHandlerTest, DynamicAndStaticParametersTest) node->get_node_graph_interface(), node->get_node_services_interface()); - auto results = rec_param->set_parameters_atomically( - {rclcpp::Parameter("dynamic_int", 10), - rclcpp::Parameter("static_int", 10)}); + std::shared_future result_future = + 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( + auto rc = rclcpp::spin_until_future_complete( node->get_node_base_interface(), - results); + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + EXPECT_EQ(result.reason, std::string("Rejected change to static parameter: ") + + "{\"name\": \"static_int\", \"type\": \"integer\", \"value\": \"10\"}"); // Now, only param1 should change, param 2 should be the same 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()); + + std::shared_future + result_future = rec_param->set_parameters_atomically({ + rclcpp::Parameter("my_node.verbose", true), + }); + + auto rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + auto result = result_future.get(); + EXPECT_EQ(result.successful, true); + EXPECT_TRUE(result.reason.empty()); + + // 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); + + // Try to set some parameters that have not been declared via the service client + result_future = rec_param->set_parameters_atomically({ + rclcpp::Parameter("static_int", 10), + rclcpp::Parameter("not_declared", true), + rclcpp::Parameter("not_declared2", true), + }); + + rc = rclcpp::spin_until_future_complete( + node->get_node_base_interface(), + result_future); + ASSERT_EQ(rclcpp::FutureReturnCode::SUCCESS, rc); + + result = result_future.get(); + EXPECT_EQ(result.successful, false); + EXPECT_FALSE(result.reason.empty()); + // The ParameterNotDeclaredException handler in rclcpp/parameter_service.cpp + // overrides any other reasons and does not provide details to the service client. + EXPECT_EQ(result.reason, std::string("One or more parameters were not declared before setting")); + + EXPECT_EQ(p1, 0); + EXPECT_EQ(p2, 0); +}