Merge pull request #307 from roveri-marco/add_parameter_for_timeout_o…

…f_plan_solver

Add parameter for timeout of plan solver

plansys2_bringup/params/plansys2_params.yaml

@@ -1,5 +1,9 @@
+planner_client:
+  ros__parameters:
+    plan_solver_timeout: 15.0  
 planner:
   ros__parameters:
+    plan_solver_timeout: 15.0  
     plan_solver_plugins: ["POPF"]
     POPF:
       plugin: "plansys2/POPFPlanSolver"

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
 {
 
@@ -52,7 +54,8 @@ class PlanSolverBase
   */
   virtual std::optional<plansys2_msgs::msg::Plan> getPlan(
     const std::string & domain, const std::string & problem,
-    const std::string & node_namespace = "") = 0;
+    const std::string & node_namespace = "",
+    const rclcpp::Duration solver_timeout = 15s) = 0;
 
   /**
    * @brief Exposes a capability to validate a PDDL domain.

plansys2_planner/include/plansys2_planner/PlannerClient.hpp

@@ -42,6 +42,7 @@ class PlannerClient : public PlannerInterface
     get_plan_client_;
 
   rclcpp::Node::SharedPtr node_;
+  rclcpp::Duration solver_timeout_ = rclcpp::Duration(15, 0);
 };
 
 }  // namespace plansys2

plansys2_planner/include/plansys2_planner/PlannerNode.hpp

@@ -74,6 +74,7 @@ class PlannerNode : public rclcpp_lifecycle::LifecycleNode
   std::vector<std::string> default_types_;
   std::vector<std::string> solver_ids_;
   std::vector<std::string> solver_types_;
+  rclcpp::Duration solver_timeout_;
 
   rclcpp::Service<plansys2_msgs::srv::GetPlan>::SharedPtr
     get_plan_service_;

plansys2_planner/src/plansys2_planner/PlannerClient.cpp

@@ -28,6 +28,15 @@ PlannerClient::PlannerClient()
   node_ = rclcpp::Node::make_shared("planner_client");
 
   get_plan_client_ = node_->create_client<plansys2_msgs::srv::GetPlan>("planner/get_plan");
+
+  double timeout;
+  node_->declare_parameter("plan_solver_timeout", 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<plansys2_msgs::msg::Plan>
@@ -44,16 +53,32 @@ PlannerClient::getPlan(
       get_plan_client_->get_service_name() <<
         " service  client: waiting for service to appear...");
   }
+  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(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<plansys2_msgs::srv::GetPlan::Request>();
   request->domain = domain;
   request->problem = problem;
 
   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 {};
   }
 

plansys2_planner/src/plansys2_planner/PlannerNode.cpp

@@ -22,16 +22,21 @@
 
 #include "lifecycle_msgs/msg/state.hpp"
 
+using namespace std::chrono_literals;
+
 namespace plansys2
 {
 
 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_);
+  double timeout = solver_timeout_.seconds();
+  declare_parameter("plan_solver_timeout", timeout);
 }
 
 
@@ -42,10 +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", timeout);
+
+  solver_timeout_ = rclcpp::Duration((int32_t)timeout, 0);
 
   if (!solver_ids_.empty()) {
     if (solver_ids_ == default_ids_) {
@@ -83,6 +92,8 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state)
       "POPF", "plansys2/POPFPlanSolver");
   }
 
+  RCLCPP_INFO(get_logger(), "[%s] Solver Timeout %g", get_name(), solver_timeout_.seconds());
+
   get_plan_service_ = create_service<plansys2_msgs::srv::GetPlan>(
     "planner/get_plan",
     std::bind(
@@ -150,8 +161,8 @@ PlannerNode::get_plan_service_callback(
   const std::shared_ptr<plansys2_msgs::srv::GetPlan::Request> request,
   const std::shared_ptr<plansys2_msgs::srv::GetPlan::Response> response)
 {
-  const auto plan = solvers_.begin()->second->getPlan(
-    request->domain, request->problem, get_namespace());
+  auto plan = solvers_.begin()->second->getPlan(
+    request->domain, request->problem, get_namespace(), solver_timeout_);
 
   if (plan) {
     response->success = true;

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
 {
 
@@ -41,7 +44,8 @@ class POPFPlanSolver : public PlanSolverBase
 
   std::optional<plansys2_msgs::msg::Plan> getPlan(
     const std::string & domain, const std::string & problem,
-    const std::string & node_namespace = "");
+    const std::string & node_namespace = "",
+    const rclcpp::Duration solver_timeout = 15s);
 
   bool isDomainValid(
     const std::string & domain,

plansys2_popf_plan_solver/src/plansys2_popf_plan_solver/popf_plan_solver.cpp

@@ -83,7 +83,8 @@ void POPFPlanSolver::configure(
 std::optional<plansys2_msgs::msg::Plan>
 POPFPlanSolver::getPlan(
   const std::string & domain, const std::string & problem,
-  const std::string & node_namespace)
+  const std::string & node_namespace,
+  const rclcpp::Duration solver_timeout)
 {
   if (system(nullptr) == 0) {
     return {};
@@ -111,6 +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.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();
   const int status = system(

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``
+