diff --git a/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp b/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp index 760a7ff6..168ebb77 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorClient.hpp @@ -18,6 +18,7 @@ #include #include #include +#include #include #include "plansys2_msgs/action/execute_plan.hpp" diff --git a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp index abd8cf0b..922ca3f1 100644 --- a/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp +++ b/plansys2_executor/include/plansys2_executor/ExecutorNode.hpp @@ -139,7 +139,7 @@ class ExecutorNode : public rclcpp_lifecycle::LifecycleNode rclcpp_action::CancelResponse handle_cancel( const std::shared_ptr goal_handle); void handle_accepted(const std::shared_ptr goal_handle); - + std::list> goal_handlers_; std::vector get_feedback_info( diff --git a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp index e929aca4..cda99d82 100644 --- a/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp +++ b/plansys2_executor/src/plansys2_executor/ExecutorNode.cpp @@ -706,7 +706,7 @@ ExecutorNode::handle_accepted(const std::shared_ptr goal_ auto result = std::make_shared(); result->result = plansys2_msgs::action::ExecutePlan::Result::FAILURE; goal_handlers_.back()->succeed(result); - + goal_handlers_.push_back(goal_handle); replan_requested_ = true; } diff --git a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp index 08be159e..8713aaeb 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp @@ -45,10 +45,11 @@ PlannerNode::~PlannerNode() for (const auto & library : loaded_libraries) { try { - lp_loader_.unloadLibraryForClass(library); - std::cout << "Successfully unloaded library: " << library << std::endl; + lp_loader_.unloadLibraryForClass(library); + std::cout << "Successfully unloaded library: " << library << std::endl; } catch (const pluginlib::LibraryUnloadException & e) { - std::cerr << "Failed to unload library: " << library << ". Error: " << e.what() << std::endl; + std::cerr << "Failed to unload library: " << library << + ". Error: " << e.what() << std::endl; } } } @@ -85,9 +86,7 @@ PlannerNode::on_configure(const rclcpp_lifecycle::State & state) plansys2::PlanSolverBase::Ptr solver = lp_loader_.createUniqueInstance(solver_types_[i]); - std::cerr << "====================== 1 " << std::endl; solver->configure(node, solver_ids_[i]); - std::cerr << "====================== 2 " << std::endl; RCLCPP_INFO( get_logger(), "Created solver : %s of type %s", @@ -183,15 +182,12 @@ PlannerNode::get_plan_service_callback( const std::shared_ptr request, const std::shared_ptr response) { - std::cerr << "get_plan_array_service_callback 1" << std::endl; std::map>> futures; std::map> results; for (auto & solver : solvers_) { - std::cerr << "get_plan_array_service_callback future for " << solver.first << std::endl; - futures[solver.first] = std::async(std::launch::async, - &plansys2::PlanSolverBase::getPlan,solver.second, + &plansys2::PlanSolverBase::getPlan, solver.second, request->domain, request->problem, get_namespace(), solver_timeout_); } @@ -202,7 +198,6 @@ PlannerNode::get_plan_service_callback( for (auto & fut : futures) { if (results.find(fut.first) == results.end()) { if (fut.second.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { - std::cerr << "get_plan_array_service_callback result ready for " << fut.first << std::endl; results[fut.first] = fut.second.get(); pending_result--; } @@ -210,15 +205,14 @@ PlannerNode::get_plan_service_callback( } } - // Destroying unfinished futures for (auto & fut : futures) { if (results.find(fut.first) == results.end()) { - std::cerr << "Destroying unfinished future for " << fut.first << std::endl; try { - // Retrieve and discard the result to avoid blocking in destructor fut.second.get(); - } catch (const std::exception &e) { - std::cerr << "Exception while destroying future for " << fut.first << ": " << e.what() << std::endl; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + get_logger(), "Exception while destroying future for " + << fut.first << ": " << e.what()); } } } @@ -227,18 +221,17 @@ PlannerNode::get_plan_service_callback( bool anyplan = false; for (auto & result : results) { if (result.second.has_value()) { - plans.push_back(result.second.value()); + plans.push_back(result.second.value()); anyplan = true; } } std::sort(plans.begin(), plans.end(), [](const plansys2_msgs::msg::Plan & a, const plansys2_msgs::msg::Plan & b) - { - return a.items.size() < b.items.size(); + { + return a.items.size() < b.items.size(); }); - std::cerr << "get_plan_array_service_callback end" << std::endl; if (anyplan) { response->success = true; response->plan = plans.front(); @@ -254,15 +247,12 @@ PlannerNode::get_plan_array_service_callback( const std::shared_ptr request, const std::shared_ptr response) { - std::cerr << "get_plan_array_service_callback 1" << std::endl; std::map>> futures; std::map> results; for (auto & solver : solvers_) { - std::cerr << "get_plan_array_service_callback future for " << solver.first << std::endl; - futures[solver.first] = std::async(std::launch::async, - &plansys2::PlanSolverBase::getPlan,solver.second, + &plansys2::PlanSolverBase::getPlan, solver.second, request->domain, request->problem, get_namespace(), solver_timeout_); } @@ -273,7 +263,6 @@ PlannerNode::get_plan_array_service_callback( for (auto & fut : futures) { if (results.find(fut.first) == results.end()) { if (fut.second.wait_for(std::chrono::seconds(0)) == std::future_status::ready) { - std::cerr << "get_plan_array_service_callback result ready for " << fut.first << std::endl; results[fut.first] = fut.second.get(); pending_result--; } @@ -284,12 +273,13 @@ PlannerNode::get_plan_array_service_callback( // Destroying unfinished futures for (auto & fut : futures) { if (results.find(fut.first) == results.end()) { - std::cerr << "Destroying unfinished future for " << fut.first << std::endl; try { // Retrieve and discard the result to avoid blocking in destructor fut.second.get(); - } catch (const std::exception &e) { - std::cerr << "Exception while destroying future for " << fut.first << ": " << e.what() << std::endl; + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + get_logger(), "Exception while destroying future for " + << fut.first << ": " << e.what()); } } } @@ -297,12 +287,11 @@ PlannerNode::get_plan_array_service_callback( bool anyplan = false; for (auto & result : results) { if (result.second.has_value()) { - response->plan_array.plan_array.push_back(result.second.value()); + response->plan_array.plan_array.push_back(result.second.value()); anyplan = true; } } - std::cerr << "get_plan_array_service_callback end" << std::endl; if (anyplan) { response->success = true; } else { diff --git a/plansys2_planner/test/unit/planner_test.cpp b/plansys2_planner/test/unit/planner_test.cpp index 7143af08..c7182773 100644 --- a/plansys2_planner/test/unit/planner_test.cpp +++ b/plansys2_planner/test/unit/planner_test.cpp @@ -134,13 +134,13 @@ TEST(planner_expert, generate_plan_with_args) planner_node->set_parameter({"plan_solver_plugins", solver_plugins}); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.plugin", + planner_node, "POPF1.plugin", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.arguments", + planner_node, "POPF1.arguments", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.output_dir", + planner_node, "POPF1.output_dir", rclcpp::ParameterValue(std::string())); planner_node->set_parameter({"POPF1.plugin", "plansys2/POPFPlanSolver"}); @@ -230,31 +230,31 @@ TEST(planner_expert, generate_plans) planner_node->set_parameter({"plan_solver_plugins", solver_plugins}); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.plugin", + planner_node, "POPF1.plugin", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.arguments", + planner_node, "POPF1.arguments", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF1.output_dir", + planner_node, "POPF1.output_dir", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF2.plugin", + planner_node, "POPF2.plugin", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF2.arguments", + planner_node, "POPF2.arguments", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "POPF2.output_dir", + planner_node, "POPF2.output_dir", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "OPTICS.plugin", + planner_node, "OPTICS.plugin", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "OPTICS.arguments", + planner_node, "OPTICS.arguments", rclcpp::ParameterValue(std::string())); plansys2::declare_parameter_if_not_declared( - planner_node, "OPTICS.output_dir", + planner_node, "OPTICS.output_dir", rclcpp::ParameterValue(std::string())); planner_node->set_parameter({"POPF1.plugin", "plansys2/POPFPlanSolver"}); @@ -319,12 +319,14 @@ TEST(planner_expert, generate_plans) ASSERT_TRUE( problem_client->setGoal(plansys2::Goal("(and (robot_talk leia message1 francisco))"))); - auto plans_bad = planner_client->getPlanArray(domain_client->getDomain(), problem_client->getProblem()); + auto plans_bad = planner_client->getPlanArray( + domain_client->getDomain(), problem_client->getProblem()); ASSERT_TRUE(plans_bad.plan_array.empty()); ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(predicates[1]))); - auto plans_good = planner_client->getPlanArray(domain_client->getDomain(), problem_client->getProblem()); + auto plans_good = planner_client->getPlanArray( + domain_client->getDomain(), problem_client->getProblem()); ASSERT_EQ(3u, plans_good.plan_array.size()); finish = true; diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp index 920db093..bb9cac1c 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpertNode.cpp @@ -588,8 +588,6 @@ ProblemExpertNode::get_problem_service_callback( } else { response->success = true; response->problem = problem_expert_->getProblem(); - - std::cerr << "get_problem_service_callback [" << response->problem << "]" << std::endl; } } diff --git a/plansys2_terminal/src/plansys2_terminal/Terminal.cpp b/plansys2_terminal/src/plansys2_terminal/Terminal.cpp index 2da4a7d3..fb5b629e 100644 --- a/plansys2_terminal/src/plansys2_terminal/Terminal.cpp +++ b/plansys2_terminal/src/plansys2_terminal/Terminal.cpp @@ -904,7 +904,6 @@ Terminal::execute_action(std::vector & command) complete_action.pop_back(); - std::cerr << "<[" << complete_action << "]" << std::endl; plansys2_msgs::msg::PlanItem action; action.time = 0.0; action.action = complete_action;