From e1f488b3c463ce66eead181dfb438f0faea6279e Mon Sep 17 00:00:00 2001 From: Marco Roveri Date: Mon, 18 Mar 2024 16:50:32 +0100 Subject: [PATCH 1/5] Added support for configuring the planner timeout --- plansys2_bringup/params/plansys2_params.yaml | 4 +++ .../include/plansys2_core/PlanSolverBase.hpp | 3 ++- .../plansys2_planner/PlannerClient.hpp | 1 + .../include/plansys2_planner/PlannerNode.hpp | 1 + .../src/plansys2_planner/PlannerClient.cpp | 26 +++++++++++++++++-- .../src/plansys2_planner/PlannerNode.cpp | 6 ++++- .../popf_plan_solver.hpp | 3 ++- .../popf_plan_solver.cpp | 6 ++++- 8 files changed, 44 insertions(+), 6 deletions(-) diff --git a/plansys2_bringup/params/plansys2_params.yaml b/plansys2_bringup/params/plansys2_params.yaml index 6422739e..6387466d 100644 --- a/plansys2_bringup/params/plansys2_params.yaml +++ b/plansys2_bringup/params/plansys2_params.yaml @@ -1,5 +1,9 @@ +planner_client: + ros__parameters: + plan_solver_timeout: 15 planner: ros__parameters: + plan_solver_timeout: 15 plan_solver_plugins: ["POPF"] POPF: plugin: "plansys2/POPFPlanSolver" diff --git a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp index ee80f78f..7b1ba391 100644 --- a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp +++ b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp @@ -38,7 +38,8 @@ class PlanSolverBase virtual std::optional getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace = "") = 0; + const std::string & node_namespace = "", + const int solver_timeout = 15) = 0; }; } // namespace plansys2 diff --git a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp index f695a902..6680a814 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp @@ -42,6 +42,7 @@ class PlannerClient : public PlannerInterface get_plan_client_; rclcpp::Node::SharedPtr node_; + int solver_timeout_ = 15; }; } // namespace plansys2 diff --git a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp index a87326ec..0b848444 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp @@ -68,6 +68,7 @@ class PlannerNode : public rclcpp_lifecycle::LifecycleNode std::vector default_types_; std::vector solver_ids_; std::vector solver_types_; + int solver_timeout_; rclcpp::Service::SharedPtr get_plan_service_; diff --git a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp index ad39e97c..84ca9737 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -28,6 +28,11 @@ PlannerClient::PlannerClient() node_ = rclcpp::Node::make_shared("planner_client"); get_plan_client_ = node_->create_client("planner/get_plan"); + + node_->declare_parameter("plan_solver_timeout", solver_timeout_); + + node_->get_parameter("plan_solver_timeout", solver_timeout_); + RCLCPP_INFO(node_->get_logger(), "Planner CLient created with timeout %d", solver_timeout_); } std::optional @@ -44,6 +49,16 @@ PlannerClient::getPlan( get_plan_client_->get_service_name() << " service client: waiting for service to appear..."); } + int timeout = solver_timeout_; + if (timeout <= 0) { + std::string timeout_str = "Get Plan service called with negative timed out:"; + timeout_str += std::to_string(solver_timeout_); + timeout_str += ". Setting to 15 seconds"; + RCLCPP_ERROR(node_->get_logger(), timeout_str.c_str()); + timeout = 15; + } + + RCLCPP_INFO(node_->get_logger(), "Get Plan service call with time out %d", timeout); auto request = std::make_shared(); request->domain = domain; @@ -51,9 +66,16 @@ PlannerClient::getPlan( auto future_result = get_plan_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(15)) != - rclcpp::FutureReturnCode::SUCCESS) + auto outresult = rclcpp::spin_until_future_complete(node_, future_result, + std::chrono::seconds(timeout)); + if (outresult != rclcpp::FutureReturnCode::SUCCESS) { + if (outresult == rclcpp::FutureReturnCode::TIMEOUT) + { + RCLCPP_ERROR(node_->get_logger(), "Get Plan service call timed out"); + } else { + RCLCPP_ERROR(node_->get_logger(), "Get Plan service call failed"); + } return {}; } diff --git a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp index 782e088b..0da7bb72 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp @@ -39,6 +39,8 @@ PlannerNode::PlannerNode() std::placeholders::_3)); declare_parameter("plan_solver_plugins", default_ids_); + solver_timeout_ = 15; + declare_parameter("plan_solver_timeout", solver_timeout_); } @@ -53,6 +55,7 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); get_parameter("plan_solver_plugins", solver_ids_); + get_parameter("plan_solver_timeout", solver_timeout_); if (!solver_ids_.empty()) { if (solver_ids_ == default_ids_) { @@ -90,6 +93,7 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) "POPF", "plansys2/POPFPlanSolver"); } + RCLCPP_INFO(get_logger(), "[%s] Solver Timeout %d", get_name(), solver_timeout_); RCLCPP_INFO(get_logger(), "[%s] Configured", get_name()); return CallbackReturnT::SUCCESS; } @@ -145,7 +149,7 @@ PlannerNode::get_plan_service_callback( const std::shared_ptr response) { auto plan = solvers_.begin()->second->getPlan( - request->domain, request->problem, get_namespace()); + request->domain, request->problem, get_namespace(), solver_timeout_); if (plan) { response->success = true; diff --git a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp index 0200d3d2..390f0237 100644 --- a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp +++ b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp @@ -37,7 +37,8 @@ class POPFPlanSolver : public PlanSolverBase std::optional getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace = ""); + const std::string & node_namespace = "", + const int solver_timeout = 15); bool is_valid_domain( const std::string & domain, diff --git a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp index 16f7b3e7..92d16829 100644 --- a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp +++ b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp @@ -44,7 +44,8 @@ void POPFPlanSolver::configure( std::optional POPFPlanSolver::getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace) + const std::string & node_namespace, + const int solver_timeout) { if (system(nullptr) == 0) { return {}; @@ -69,6 +70,9 @@ POPFPlanSolver::getPlan( problem_out << problem; problem_out.close(); + RCLCPP_INFO(lc_node_->get_logger(), "[%s-popf] called with timeout %d seconds", + lc_node_->get_name(), solver_timeout); + int status = system( ("ros2 run popf popf " + lc_node_->get_parameter(parameter_name_).value_to_string() + From 0bd2571852a09635b1e65f8d10dfe4ee53c4dbe9 Mon Sep 17 00:00:00 2001 From: Marco Roveri Date: Mon, 18 Mar 2024 16:51:11 +0100 Subject: [PATCH 2/5] Added some information in the README to allow for using a configuration file to change the planner timeout --- plansys2_terminal/README.md | 3 +++ 1 file changed, 3 insertions(+) diff --git a/plansys2_terminal/README.md b/plansys2_terminal/README.md index 8fc828f6..e94c3c23 100644 --- a/plansys2_terminal/README.md +++ b/plansys2_terminal/README.md @@ -4,3 +4,6 @@ The Plansys2 Terminal module is a tool to directly monitor and edit your applica The node is created inside a Terminal object, which starts a looping run_console() method reading the terminal input and calling the corresponding services and actions. +To enable for using parameters specified in a YAML file for e.g. specifying the timeout for the planner and planner client, you can use the following command: +``ros2 run plansys2_terminal plansys2_terminal --ros-args --params-file `ros2 pkg prefix --share plansys2_bringup`/params/plansys2_params.yaml`` + From be4fba4a607af032ed16a995db10f84364b9b008 Mon Sep 17 00:00:00 2001 From: Marco Roveri Date: Wed, 3 Apr 2024 13:47:32 +0200 Subject: [PATCH 3/5] Fixed coding styles --- .../src/plansys2_planner/PlannerClient.cpp | 13 ++++++------- 1 file changed, 6 insertions(+), 7 deletions(-) diff --git a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp index 84ca9737..8b4b1f06 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -30,7 +30,7 @@ PlannerClient::PlannerClient() get_plan_client_ = node_->create_client("planner/get_plan"); node_->declare_parameter("plan_solver_timeout", solver_timeout_); - + node_->get_parameter("plan_solver_timeout", solver_timeout_); RCLCPP_INFO(node_->get_logger(), "Planner CLient created with timeout %d", solver_timeout_); } @@ -66,12 +66,11 @@ PlannerClient::getPlan( auto future_result = get_plan_client_->async_send_request(request); - auto outresult = rclcpp::spin_until_future_complete(node_, future_result, - std::chrono::seconds(timeout)); - if (outresult != rclcpp::FutureReturnCode::SUCCESS) - { - if (outresult == rclcpp::FutureReturnCode::TIMEOUT) - { + auto outresult = rclcpp::spin_until_future_complete( + node_, future_result, + std::chrono::seconds(timeout)); + if (outresult != rclcpp::FutureReturnCode::SUCCESS) { + if (outresult == rclcpp::FutureReturnCode::TIMEOUT) { RCLCPP_ERROR(node_->get_logger(), "Get Plan service call timed out"); } else { RCLCPP_ERROR(node_->get_logger(), "Get Plan service call failed"); From 03826123ded140adbdf35cb29bfeee3474f88f29 Mon Sep 17 00:00:00 2001 From: Marco Roveri Date: Thu, 4 Apr 2024 00:09:07 +0200 Subject: [PATCH 4/5] Fixing coding style --- plansys2_planner/src/plansys2_planner/PlannerClient.cpp | 4 ++-- .../src/plansys2_popf_plan_solver/popf_plan_solver.cpp | 7 ++++--- 2 files changed, 6 insertions(+), 5 deletions(-) diff --git a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp index 8b4b1f06..0287204a 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -67,8 +67,8 @@ PlannerClient::getPlan( auto future_result = get_plan_client_->async_send_request(request); auto outresult = rclcpp::spin_until_future_complete( - node_, future_result, - std::chrono::seconds(timeout)); + node_, future_result, + std::chrono::seconds(timeout)); if (outresult != rclcpp::FutureReturnCode::SUCCESS) { if (outresult == rclcpp::FutureReturnCode::TIMEOUT) { RCLCPP_ERROR(node_->get_logger(), "Get Plan service call timed out"); diff --git a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp index 3919ab7b..c3c30d9c 100644 --- a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp +++ b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp @@ -112,9 +112,10 @@ POPFPlanSolver::getPlan( problem_out << problem; problem_out.close(); - RCLCPP_INFO(lc_node_->get_logger(), "[%s-popf] called with timeout %d seconds", - lc_node_->get_name(), solver_timeout); - + RCLCPP_INFO( + lc_node_->get_logger(), "[%s-popf] called with timeout %d seconds", + lc_node_->get_name(), solver_timeout); + const auto plan_file_path = output_dir / std::filesystem::path("plan"); const auto args = lc_node_->get_parameter(arguments_parameter_name_).value_to_string(); const int status = system( From 5a552515271e64a96d65819de3b29c7f174d543e Mon Sep 17 00:00:00 2001 From: Marco Roveri Date: Fri, 5 Apr 2024 14:35:10 +0200 Subject: [PATCH 5/5] Integrated feedback, and fixes to have tests to pass --- plansys2_bringup/params/plansys2_params.yaml | 4 ++-- .../include/plansys2_core/PlanSolverBase.hpp | 4 +++- .../include/plansys2_planner/PlannerClient.hpp | 2 +- .../include/plansys2_planner/PlannerNode.hpp | 2 +- .../src/plansys2_planner/PlannerClient.cpp | 14 +++++++++----- .../src/plansys2_planner/PlannerNode.cpp | 16 +++++++++++----- .../popf_plan_solver.hpp | 5 ++++- .../popf_plan_solver.cpp | 4 ++-- 8 files changed, 33 insertions(+), 18 deletions(-) diff --git a/plansys2_bringup/params/plansys2_params.yaml b/plansys2_bringup/params/plansys2_params.yaml index 6387466d..dae4ded4 100644 --- a/plansys2_bringup/params/plansys2_params.yaml +++ b/plansys2_bringup/params/plansys2_params.yaml @@ -1,9 +1,9 @@ planner_client: ros__parameters: - plan_solver_timeout: 15 + plan_solver_timeout: 15.0 planner: ros__parameters: - plan_solver_timeout: 15 + plan_solver_timeout: 15.0 plan_solver_plugins: ["POPF"] POPF: plugin: "plansys2/POPFPlanSolver" diff --git a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp index 78dd2ed2..3e61a6bd 100644 --- a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp +++ b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp @@ -24,6 +24,8 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" +using namespace std::chrono_literals; + namespace plansys2 { @@ -53,7 +55,7 @@ class PlanSolverBase virtual std::optional getPlan( const std::string & domain, const std::string & problem, const std::string & node_namespace = "", - const int solver_timeout = 15) = 0; + const rclcpp::Duration solver_timeout = 15s) = 0; /** * @brief Exposes a capability to validate a PDDL domain. diff --git a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp index 6680a814..502b2439 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp @@ -42,7 +42,7 @@ class PlannerClient : public PlannerInterface get_plan_client_; rclcpp::Node::SharedPtr node_; - int solver_timeout_ = 15; + rclcpp::Duration solver_timeout_ = rclcpp::Duration(15, 0); }; } // namespace plansys2 diff --git a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp index 2e2cc903..d4863ebb 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp @@ -74,7 +74,7 @@ class PlannerNode : public rclcpp_lifecycle::LifecycleNode std::vector default_types_; std::vector solver_ids_; std::vector solver_types_; - int solver_timeout_; + rclcpp::Duration solver_timeout_; rclcpp::Service::SharedPtr get_plan_service_; diff --git a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp index 0287204a..019b9c90 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -29,10 +29,14 @@ PlannerClient::PlannerClient() get_plan_client_ = node_->create_client("planner/get_plan"); - node_->declare_parameter("plan_solver_timeout", solver_timeout_); + double timeout; + node_->declare_parameter("plan_solver_timeout", timeout); - node_->get_parameter("plan_solver_timeout", solver_timeout_); - RCLCPP_INFO(node_->get_logger(), "Planner CLient created with timeout %d", solver_timeout_); + node_->get_parameter("plan_solver_timeout", timeout); + solver_timeout_ = rclcpp::Duration((int32_t)timeout, 0); + RCLCPP_INFO( + node_->get_logger(), "Planner CLient created with timeout %g", + solver_timeout_.seconds()); } std::optional @@ -49,10 +53,10 @@ PlannerClient::getPlan( get_plan_client_->get_service_name() << " service client: waiting for service to appear..."); } - int timeout = solver_timeout_; + int32_t timeout = solver_timeout_.seconds(); if (timeout <= 0) { std::string timeout_str = "Get Plan service called with negative timed out:"; - timeout_str += std::to_string(solver_timeout_); + timeout_str += std::to_string(timeout); timeout_str += ". Setting to 15 seconds"; RCLCPP_ERROR(node_->get_logger(), timeout_str.c_str()); timeout = 15; diff --git a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp index 10a7e567..b807a230 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp @@ -22,6 +22,8 @@ #include "lifecycle_msgs/msg/state.hpp" +using namespace std::chrono_literals; + namespace plansys2 { @@ -29,11 +31,12 @@ PlannerNode::PlannerNode() : rclcpp_lifecycle::LifecycleNode("planner"), lp_loader_("plansys2_core", "plansys2::PlanSolverBase"), default_ids_{}, - default_types_{} + default_types_{}, + solver_timeout_(15s) { declare_parameter("plan_solver_plugins", default_ids_); - solver_timeout_ = 15; - declare_parameter("plan_solver_timeout", solver_timeout_); + double timeout = solver_timeout_.seconds(); + declare_parameter("plan_solver_timeout", timeout); } @@ -44,11 +47,14 @@ CallbackReturnT PlannerNode::on_configure(const rclcpp_lifecycle::State & state) { auto node = shared_from_this(); + double timeout; RCLCPP_INFO(get_logger(), "[%s] Configuring...", get_name()); get_parameter("plan_solver_plugins", solver_ids_); - get_parameter("plan_solver_timeout", solver_timeout_); + get_parameter("plan_solver_timeout", timeout); + + solver_timeout_ = rclcpp::Duration((int32_t)timeout, 0); if (!solver_ids_.empty()) { if (solver_ids_ == default_ids_) { @@ -86,7 +92,7 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) "POPF", "plansys2/POPFPlanSolver"); } - RCLCPP_INFO(get_logger(), "[%s] Solver Timeout %d", get_name(), solver_timeout_); + RCLCPP_INFO(get_logger(), "[%s] Solver Timeout %g", get_name(), solver_timeout_.seconds()); get_plan_service_ = create_service( "planner/get_plan", diff --git a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp index da258f5b..11aa52a4 100644 --- a/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp +++ b/plansys2_popf_plan_solver/include/plansys2_popf_plan_solver/popf_plan_solver.hpp @@ -22,6 +22,9 @@ #include "plansys2_core/PlanSolverBase.hpp" +// using namespace std::chrono_literals; +using std::chrono_literals::operator""s; + namespace plansys2 { @@ -42,7 +45,7 @@ class POPFPlanSolver : public PlanSolverBase std::optional getPlan( const std::string & domain, const std::string & problem, const std::string & node_namespace = "", - const int solver_timeout = 15); + const rclcpp::Duration solver_timeout = 15s); bool isDomainValid( const std::string & domain, diff --git a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp index c3c30d9c..4801043c 100644 --- a/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp +++ b/plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp @@ -84,7 +84,7 @@ std::optional POPFPlanSolver::getPlan( const std::string & domain, const std::string & problem, const std::string & node_namespace, - const int solver_timeout) + const rclcpp::Duration solver_timeout) { if (system(nullptr) == 0) { return {}; @@ -114,7 +114,7 @@ POPFPlanSolver::getPlan( RCLCPP_INFO( lc_node_->get_logger(), "[%s-popf] called with timeout %d seconds", - lc_node_->get_name(), solver_timeout); + lc_node_->get_name(), solver_timeout.seconds()); const auto plan_file_path = output_dir / std::filesystem::path("plan"); const auto args = lc_node_->get_parameter(arguments_parameter_name_).value_to_string();