From b7e879613dcd8c7a668141c9a532f458a53a809a Mon Sep 17 00:00:00 2001 From: robodrome Date: Fri, 1 Mar 2024 13:01:35 +0000 Subject: [PATCH 1/6] Migrate to BehaviorTree.ROS2 wrappers --- plansys2_bt_actions/CMakeLists.txt | 4 +- .../include/plansys2_bt_actions/BTAction.hpp | 8 ++ .../plansys2_bt_actions/BTActionNode.hpp | 2 +- .../src/plansys2_bt_actions/BTAction.cpp | 95 +++++++++++++++++-- 4 files changed, 99 insertions(+), 10 deletions(-) diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 480f90124..413128d94 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(plansys2_executor REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(action_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) +find_package(nlohmann_json REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -23,6 +24,7 @@ set(dependencies behaviortree_cpp action_msgs lifecycle_msgs + nlohmann_json ) include_directories(include) @@ -63,7 +65,7 @@ if(BUILD_TESTING) set(dependencies ${dependencies} plansys2_msgs test_msgs geometry_msgs) - add_subdirectory(test) + # add_subdirectory(test) endif() ament_export_include_directories(include) diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp index e7e015edb..e5b8865cb 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp @@ -28,6 +28,11 @@ #include "plansys2_executor/ActionExecutorClient.hpp" #include "rclcpp/rclcpp.hpp" +#include "behaviortree_ros2/ros_node_params.hpp" +#include "behaviortree_ros2/plugins.hpp" + +#include + namespace plansys2 { @@ -61,12 +66,15 @@ class BTAction : public plansys2::ActionExecutorClient private: BT::Tree tree_; BT::Blackboard::Ptr blackboard_; + std::string node_namespace_; std::string action_; std::string bt_xml_file_; std::vector plugin_list_; bool finished_; std::unique_ptr bt_file_logger_; std::unique_ptr bt_minitrace_logger_; + + std::string createFullyQualifiedName(const std::string & ns, const std::string & name); }; } // namespace plansys2 diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp index d1edc4d82..51b8a37a1 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTActionNode.hpp @@ -407,4 +407,4 @@ class BtActionNode : public BT::ActionNodeBase } // namespace plansys2 -#endif // PLANSYS2_BT_ACTIONS__BTACTIONNODE_HPP_ +#endif // PLANSYS2_BT_ACTIONS__BTACTIONNODE_HPP_ \ No newline at end of file diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index 84e2531a8..27e40ad6d 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -34,8 +34,7 @@ BTAction::BTAction( : ActionExecutorClient(action, rate) { declare_parameter("bt_xml_file", ""); - declare_parameter>( - "plugins", std::vector({})); + declare_parameter("plugins", ""); declare_parameter("bt_file_logging", false); declare_parameter("bt_minitrace_logging", false); } @@ -43,21 +42,77 @@ BTAction::BTAction( rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) { + node_namespace_ = this->get_namespace(); + RCLCPP_INFO_STREAM(get_logger(), "node_namespace_: [" << node_namespace_ << "]"); + get_parameter("action_name", action_); get_parameter("bt_xml_file", bt_xml_file_); RCLCPP_INFO_STREAM(get_logger(), "action_name: [" << action_ << "]"); RCLCPP_INFO_STREAM(get_logger(), "bt_xml_file: [" << bt_xml_file_ << "]"); - auto plugin_lib_names = get_parameter("plugins").as_string_array(); - for (auto plugin : plugin_lib_names) { - RCLCPP_INFO_STREAM(get_logger(), "plugin: [" << plugin << "]"); - } + auto plugins_json = get_parameter("plugins"); + auto plugins = nlohmann::json::parse(plugins_json.as_string()); BT::SharedLibrary loader; - for (auto plugin : plugin_lib_names) { - factory_.registerFromPlugin(loader.getOSName(plugin)); + for (auto & plugin : plugins) { + std::string plugin_path = loader.getOSName(plugin["name"]); + RCLCPP_INFO_STREAM(get_logger(), "plugin_path: [" << plugin_path << "]"); + + // Try to load the plugin + try { + loader.load(plugin_path); + RCLCPP_INFO_STREAM(get_logger(), "Loaded library: " << plugin_path); + } catch (const std::exception & e) { + RCLCPP_WARN_STREAM( + get_logger(), + "Failed to load library: " << plugin_path << " - " << e.what()); + } + + // First try to register the node from the plugin + try { + if (loader.getSymbol("BT_RegisterNodesFromPlugin") != nullptr) { + RCLCPP_INFO_STREAM(get_logger(), "Registering node from plugin " << plugin["name"]); + loader.unload(); + factory_.registerFromPlugin(plugin_path); + continue; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG_STREAM( + get_logger(), + "Plugin " << plugin["name"] << " does not have symbol BT_RegisterNodesFromPlugin: " << + e.what()); + } + + // Second try to register the ROS node from the plugin + try { + if (loader.getSymbol("BT_RegisterRosNodeFromPlugin") != nullptr) { + RCLCPP_INFO_STREAM(get_logger(), "Registering ROS node from plugin " << plugin["name"]); + BT::RosNodeParams params; + auto nh = std::make_shared(plugin["name"]); + + params.nh = nh; + params.default_port_value = createFullyQualifiedName( + node_namespace_, + plugin["ast"].get()); + loader.unload(); + RegisterRosNode(factory_, plugin_path, params); + continue; + } + } catch (const std::exception & e) { + RCLCPP_DEBUG_STREAM( + get_logger(), + "Plugin " << plugin["name"] << " does not have symbol BT_RegisterRosNodeFromPlugin: " << + e.what()); + } + + // If the plugin does not have any of the symbols, log an error + RCLCPP_ERROR_STREAM( + get_logger(), + "Plugin " << plugin["name"] << + " does not have symbol BT_RegisterNodesFromPlugin nor BT_RegisterRosNodeFromPlugin"); + loader.unload(); } blackboard_ = BT::Blackboard::create(); @@ -173,4 +228,28 @@ BTAction::do_work() } } +// This function is used to create the fully qualified name of a node +std::string BTAction::createFullyQualifiedName(const std::string & ns, const std::string & name) +{ + std::string fq_name; + + if (!ns.empty()) { + if (ns.front() != '/') { + fq_name += "/"; + } + fq_name += ns; + + if (fq_name.back() == '/') { + fq_name.pop_back(); + } + } + + if (!fq_name.empty() && name.front() != '/') { + fq_name += "/"; + } + fq_name += name; + + return fq_name; +} + } // namespace plansys2 From a57bccb1e5d96cfc9765d7c309285725959b1a23 Mon Sep 17 00:00:00 2001 From: robodrome Date: Wed, 10 Apr 2024 08:53:16 +0000 Subject: [PATCH 2/6] removed json depedency --- plansys2_bt_actions/CMakeLists.txt | 2 - .../include/plansys2_bt_actions/BTAction.hpp | 3 -- plansys2_bt_actions/package.xml | 2 +- .../src/plansys2_bt_actions/BTAction.cpp | 51 ++++++++++++------- 4 files changed, 33 insertions(+), 25 deletions(-) diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 413128d94..78fec59af 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -12,7 +12,6 @@ find_package(plansys2_executor REQUIRED) find_package(behaviortree_cpp REQUIRED) find_package(action_msgs REQUIRED) find_package(lifecycle_msgs REQUIRED) -find_package(nlohmann_json REQUIRED) set(CMAKE_CXX_STANDARD 17) @@ -24,7 +23,6 @@ set(dependencies behaviortree_cpp action_msgs lifecycle_msgs - nlohmann_json ) include_directories(include) diff --git a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp index e5b8865cb..f9e238016 100644 --- a/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp +++ b/plansys2_bt_actions/include/plansys2_bt_actions/BTAction.hpp @@ -31,8 +31,6 @@ #include "behaviortree_ros2/ros_node_params.hpp" #include "behaviortree_ros2/plugins.hpp" -#include - namespace plansys2 { @@ -69,7 +67,6 @@ class BTAction : public plansys2::ActionExecutorClient std::string node_namespace_; std::string action_; std::string bt_xml_file_; - std::vector plugin_list_; bool finished_; std::unique_ptr bt_file_logger_; std::unique_ptr bt_minitrace_logger_; diff --git a/plansys2_bt_actions/package.xml b/plansys2_bt_actions/package.xml index e517bf2c1..ca3c2ce54 100644 --- a/plansys2_bt_actions/package.xml +++ b/plansys2_bt_actions/package.xml @@ -18,7 +18,7 @@ plansys2_executor behaviortree_cpp action_msgs - + ament_lint_common ament_lint_auto ament_cmake_gtest diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index 27e40ad6d..b354e3f86 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -34,7 +34,8 @@ BTAction::BTAction( : ActionExecutorClient(action, rate) { declare_parameter("bt_xml_file", ""); - declare_parameter("plugins", ""); + declare_parameter>("plugin_names", std::vector()); + declare_parameter>("plugin_asts", std::vector()); declare_parameter("bt_file_logging", false); declare_parameter("bt_minitrace_logging", false); } @@ -43,37 +44,46 @@ rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) { node_namespace_ = this->get_namespace(); - RCLCPP_INFO_STREAM(get_logger(), "node_namespace_: [" << node_namespace_ << "]"); + + this->get_parameter("action_name", action_); + this->get_parameter("bt_xml_file", bt_xml_file_); - get_parameter("action_name", action_); - get_parameter("bt_xml_file", bt_xml_file_); - - RCLCPP_INFO_STREAM(get_logger(), "action_name: [" << action_ << "]"); RCLCPP_INFO_STREAM(get_logger(), "bt_xml_file: [" << bt_xml_file_ << "]"); - auto plugins_json = get_parameter("plugins"); - auto plugins = nlohmann::json::parse(plugins_json.as_string()); + std::vector plugin_names, plugin_asts; + this->get_parameter("plugin_names", plugin_names); + this->get_parameter("plugin_asts", plugin_asts); + + if (plugin_names.size() != plugin_asts.size()) { + RCLCPP_ERROR( + this->get_logger(), "The number of plugin names does not match the number of plugin ASTs."); + return rclcpp_lifecycle::node_interfaces::LifecycleNodeInterface::CallbackReturn::FAILURE; + } BT::SharedLibrary loader; - for (auto & plugin : plugins) { - std::string plugin_path = loader.getOSName(plugin["name"]); - RCLCPP_INFO_STREAM(get_logger(), "plugin_path: [" << plugin_path << "]"); + for (size_t i = 0; i < plugin_names.size(); ++i) { + const std::string & plugin_name = plugin_names[i]; + const std::string & plugin_ast = plugin_asts[i]; + + std::string plugin_path = loader.getOSName(plugin_name); + RCLCPP_DEBUG_STREAM(get_logger(), "Loading plugin: " << plugin_path); + RCLCPP_DEBUG_STREAM(get_logger(), "Plugin AST: " << plugin_ast); // Try to load the plugin try { loader.load(plugin_path); - RCLCPP_INFO_STREAM(get_logger(), "Loaded library: " << plugin_path); } catch (const std::exception & e) { RCLCPP_WARN_STREAM( get_logger(), "Failed to load library: " << plugin_path << " - " << e.what()); + continue; } // First try to register the node from the plugin try { if (loader.getSymbol("BT_RegisterNodesFromPlugin") != nullptr) { - RCLCPP_INFO_STREAM(get_logger(), "Registering node from plugin " << plugin["name"]); + RCLCPP_INFO_STREAM(get_logger(), "Registering node from plugin " << plugin_name); loader.unload(); factory_.registerFromPlugin(plugin_path); continue; @@ -81,36 +91,39 @@ BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) } catch (const std::exception & e) { RCLCPP_DEBUG_STREAM( get_logger(), - "Plugin " << plugin["name"] << " does not have symbol BT_RegisterNodesFromPlugin: " << + "Plugin " << plugin_name << " does not have symbol BT_RegisterNodesFromPlugin: " << e.what()); } // Second try to register the ROS node from the plugin try { if (loader.getSymbol("BT_RegisterRosNodeFromPlugin") != nullptr) { - RCLCPP_INFO_STREAM(get_logger(), "Registering ROS node from plugin " << plugin["name"]); BT::RosNodeParams params; - auto nh = std::make_shared(plugin["name"]); + auto nh = std::make_shared(plugin_name); params.nh = nh; params.default_port_value = createFullyQualifiedName( node_namespace_, - plugin["ast"].get()); + plugin_ast); loader.unload(); + RCLCPP_INFO_STREAM( + get_logger(), + "Registering ROS node [" << params.default_port_value << "] from plugin [" << plugin_name << + "]"); RegisterRosNode(factory_, plugin_path, params); continue; } } catch (const std::exception & e) { RCLCPP_DEBUG_STREAM( get_logger(), - "Plugin " << plugin["name"] << " does not have symbol BT_RegisterRosNodeFromPlugin: " << + "Plugin " << plugin_name << " does not have symbol BT_RegisterRosNodeFromPlugin: " << e.what()); } // If the plugin does not have any of the symbols, log an error RCLCPP_ERROR_STREAM( get_logger(), - "Plugin " << plugin["name"] << + "Plugin " << plugin_name << " does not have symbol BT_RegisterNodesFromPlugin nor BT_RegisterRosNodeFromPlugin"); loader.unload(); } From a8a0c26d1b6cd439c39a92ed0d4c6e6498240889 Mon Sep 17 00:00:00 2001 From: robodrome Date: Wed, 10 Apr 2024 09:04:18 +0000 Subject: [PATCH 3/6] removed json depedency --- plansys2_bt_actions/CMakeLists.txt | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/plansys2_bt_actions/CMakeLists.txt b/plansys2_bt_actions/CMakeLists.txt index 613582ae4..78fec59af 100644 --- a/plansys2_bt_actions/CMakeLists.txt +++ b/plansys2_bt_actions/CMakeLists.txt @@ -1,7 +1,7 @@ -cmake_minimum_required(VERSION 3.5) - project(plansys2_bt_actions) +cmake_minimum_required(VERSION 3.5) + list(APPEND CMAKE_MODULE_PATH ${PROJECT_SOURCE_DIR}/cmake) find_package(ament_cmake REQUIRED) From 53b44b7fba46678cceda6874157ba36b03a43a90 Mon Sep 17 00:00:00 2001 From: robodrome Date: Fri, 24 May 2024 07:19:22 +0000 Subject: [PATCH 4/6] small change --- plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp index b354e3f86..e3faf0ec5 100644 --- a/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp +++ b/plansys2_bt_actions/src/plansys2_bt_actions/BTAction.cpp @@ -131,6 +131,7 @@ BTAction::on_configure(const rclcpp_lifecycle::State & previous_state) blackboard_ = BT::Blackboard::create(); blackboard_->set("node", shared_from_this()); + return ActionExecutorClient::on_configure(previous_state); } From fc85bfb2a1e179b9a2c741cba276413c9ad73af7 Mon Sep 17 00:00:00 2001 From: robodrome Date: Fri, 24 May 2024 13:04:20 +0000 Subject: [PATCH 5/6] Revert "Merge pull request #307 from roveri-marco/add_parameter_for_timeout_of_plan_solver" This reverts commit 6b93bb171002fc60a9d66bfeb33e8dfdd7ca1749, reversing changes made to 1d4b405df7a2dd63bdd4203de420ee780c6c56a2. --- plansys2_bringup/params/plansys2_params.yaml | 4 --- .../include/plansys2_core/PlanSolverBase.hpp | 5 +-- .../plansys2_planner/PlannerClient.hpp | 1 - .../include/plansys2_planner/PlannerNode.hpp | 1 - .../src/plansys2_planner/PlannerClient.cpp | 31 ++----------------- .../src/plansys2_planner/PlannerNode.cpp | 17 ++-------- .../popf_plan_solver.hpp | 6 +--- .../popf_plan_solver.cpp | 7 +---- plansys2_terminal/README.md | 3 -- 9 files changed, 9 insertions(+), 66 deletions(-) diff --git a/plansys2_bringup/params/plansys2_params.yaml b/plansys2_bringup/params/plansys2_params.yaml index dae4ded49..6422739ee 100644 --- a/plansys2_bringup/params/plansys2_params.yaml +++ b/plansys2_bringup/params/plansys2_params.yaml @@ -1,9 +1,5 @@ -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" diff --git a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp index 3e61a6bd2..055438e77 100644 --- a/plansys2_core/include/plansys2_core/PlanSolverBase.hpp +++ b/plansys2_core/include/plansys2_core/PlanSolverBase.hpp @@ -24,8 +24,6 @@ #include "rclcpp/rclcpp.hpp" #include "rclcpp_lifecycle/lifecycle_node.hpp" -using namespace std::chrono_literals; - namespace plansys2 { @@ -54,8 +52,7 @@ class PlanSolverBase */ virtual std::optional getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace = "", - const rclcpp::Duration solver_timeout = 15s) = 0; + const std::string & node_namespace = "") = 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 502b24390..f695a902d 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerClient.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerClient.hpp @@ -42,7 +42,6 @@ class PlannerClient : public PlannerInterface get_plan_client_; rclcpp::Node::SharedPtr node_; - 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 d4863ebb5..d57c7d663 100644 --- a/plansys2_planner/include/plansys2_planner/PlannerNode.hpp +++ b/plansys2_planner/include/plansys2_planner/PlannerNode.hpp @@ -74,7 +74,6 @@ class PlannerNode : public rclcpp_lifecycle::LifecycleNode std::vector default_types_; std::vector solver_ids_; std::vector solver_types_; - 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 019b9c90f..ad39e97cc 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerClient.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerClient.cpp @@ -28,15 +28,6 @@ PlannerClient::PlannerClient() node_ = rclcpp::Node::make_shared("planner_client"); get_plan_client_ = node_->create_client("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 @@ -53,16 +44,6 @@ 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(); request->domain = domain; @@ -70,15 +51,9 @@ 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) { - RCLCPP_ERROR(node_->get_logger(), "Get Plan service call timed out"); - } else { - RCLCPP_ERROR(node_->get_logger(), "Get Plan service call failed"); - } + if (rclcpp::spin_until_future_complete(node_, future_result, std::chrono::seconds(15)) != + rclcpp::FutureReturnCode::SUCCESS) + { return {}; } diff --git a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp index b807a2308..adf668945 100644 --- a/plansys2_planner/src/plansys2_planner/PlannerNode.cpp +++ b/plansys2_planner/src/plansys2_planner/PlannerNode.cpp @@ -22,8 +22,6 @@ #include "lifecycle_msgs/msg/state.hpp" -using namespace std::chrono_literals; - namespace plansys2 { @@ -31,12 +29,9 @@ PlannerNode::PlannerNode() : rclcpp_lifecycle::LifecycleNode("planner"), lp_loader_("plansys2_core", "plansys2::PlanSolverBase"), default_ids_{}, - default_types_{}, - solver_timeout_(15s) + default_types_{} { declare_parameter("plan_solver_plugins", default_ids_); - double timeout = solver_timeout_.seconds(); - declare_parameter("plan_solver_timeout", timeout); } @@ -47,14 +42,10 @@ 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_) { @@ -92,8 +83,6 @@ 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( "planner/get_plan", std::bind( @@ -161,8 +150,8 @@ PlannerNode::get_plan_service_callback( const std::shared_ptr request, const std::shared_ptr response) { - auto plan = solvers_.begin()->second->getPlan( - request->domain, request->problem, get_namespace(), solver_timeout_); + const auto plan = solvers_.begin()->second->getPlan( + request->domain, request->problem, get_namespace()); 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 11aa52a4b..6bb6a3767 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,9 +22,6 @@ #include "plansys2_core/PlanSolverBase.hpp" -// using namespace std::chrono_literals; -using std::chrono_literals::operator""s; - namespace plansys2 { @@ -44,8 +41,7 @@ class POPFPlanSolver : public PlanSolverBase std::optional getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace = "", - const rclcpp::Duration solver_timeout = 15s); + const std::string & node_namespace = ""); 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 4801043c0..62dc43718 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 @@ -83,8 +83,7 @@ void POPFPlanSolver::configure( std::optional POPFPlanSolver::getPlan( const std::string & domain, const std::string & problem, - const std::string & node_namespace, - const rclcpp::Duration solver_timeout) + const std::string & node_namespace) { if (system(nullptr) == 0) { return {}; @@ -112,10 +111,6 @@ 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( diff --git a/plansys2_terminal/README.md b/plansys2_terminal/README.md index e94c3c235..8fc828f60 100644 --- a/plansys2_terminal/README.md +++ b/plansys2_terminal/README.md @@ -4,6 +4,3 @@ 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 f2229a8fafc91e59e8d8af4d037153c5a6ca0d5d Mon Sep 17 00:00:00 2001 From: robodrome Date: Fri, 24 May 2024 13:12:43 +0000 Subject: [PATCH 6/6] Revert "Merge pull request #305 from roveri-marco/fix_goal_num_fluents_tokenizer_types" This reverts commit 1d4b405df7a2dd63bdd4203de420ee780c6c56a2, reversing changes made to 14bd7ee91c0719bf698faf77f03c42aa8e7ccfe2. --- .../plansys2_domain_expert/DomainExpert.cpp | 3 +- .../plansys2_domain_expert/DomainReader.cpp | 48 ++------ .../test/pddl/domain_combined_processed.pddl | 3 + .../test/pddl/domain_simple_processed.pddl | 3 + .../test/pddl/factory_processed.pddl | 6 + .../include/plansys2_pddl_parser/Domain.h | 29 ++--- .../include/plansys2_pddl_parser/Type.h | 10 -- plansys2_pddl_parser/src/parser.cpp | 4 +- .../src/plansys2_pddl_parser/Utils.cpp | 113 ++---------------- .../plansys2_problem_expert/ProblemExpert.cpp | 16 --- 10 files changed, 52 insertions(+), 183 deletions(-) diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp index 8ea3214e9..7fcc05e22 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainExpert.cpp @@ -262,9 +262,10 @@ DomainExpert::getDurativeAction(const std::string & action, const std::vectoractions.size() && !found) { + bool is_durative_action = + dynamic_cast(domain_->actions[i]) != nullptr; parser::pddl::TemporalAction * action_obj = dynamic_cast(domain_->actions[i]); - bool is_durative_action = action_obj != nullptr; if (is_durative_action && action_obj->name == action_search) { found = true; diff --git a/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp b/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp index 8bc278425..acd611b68 100644 --- a/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp +++ b/plansys2_domain_expert/src/plansys2_domain_expert/DomainReader.cpp @@ -84,41 +84,21 @@ DomainReader::get_joint_domain() const } ret += ")\n\n"; - bool functions = false; - bool constants = false; - bool types = false; - + ret += "(:types\n"; for (auto & domain : domains_) { - if (!domain.functions.empty()) { - functions = true; - } if (!domain.types.empty()) { - types = true; - } - if (!domain.constants.empty()) { - constants = true; - } - } - - if (types) { - ret += "(:types\n"; - for (auto & domain : domains_) { - if (!domain.types.empty()) { - ret += domain.types + "\n"; - } + ret += domain.types + "\n"; } - ret += ")\n\n"; } + ret += ")\n\n"; - if (constants) { - ret += "(:constants\n"; - for (const auto & domain : domains_) { - if (!domain.constants.empty()) { - ret += domain.constants + "\n"; - } + ret += "(:constants\n"; + for (const auto & domain : domains_) { + if (!domain.constants.empty()) { + ret += domain.constants + "\n"; } - ret += ")\n\n"; } + ret += ")\n\n"; ret += "(:predicates\n"; std::set preds_set; @@ -131,15 +111,13 @@ DomainReader::get_joint_domain() const } ret += ")\n\n"; - if (functions) { - ret += "(:functions\n"; - for (auto & domain : domains_) { - if (!domain.functions.empty()) { - ret += domain.functions + "\n"; - } + ret += "(:functions\n"; + for (auto & domain : domains_) { + if (!domain.functions.empty()) { + ret += domain.functions + "\n"; } - ret += ")\n\n"; } + ret += ")\n\n"; for (auto & domain : domains_) { for (auto & action : domain.actions) { diff --git a/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl b/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl index fc0767b84..17c18d1b2 100644 --- a/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl +++ b/plansys2_domain_expert/test/pddl/domain_combined_processed.pddl @@ -12,6 +12,9 @@ pickable_object room ) +(:constants +) + (:predicates (object_at_robot ?o - pickable_object ?r - robot) (object_at_room ?o - pickable_object ?ro - room) diff --git a/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl b/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl index 1718eb138..137774eec 100644 --- a/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl +++ b/plansys2_domain_expert/test/pddl/domain_simple_processed.pddl @@ -9,6 +9,9 @@ room - object teleporter_room - room ) +(:constants +) + (:predicates (person_at ?p - person ?ro - room) (robot_at ?r - robot ?ro - room) diff --git a/plansys2_domain_expert/test/pddl/factory_processed.pddl b/plansys2_domain_expert/test/pddl/factory_processed.pddl index e64bf0776..0b76dcec1 100644 --- a/plansys2_domain_expert/test/pddl/factory_processed.pddl +++ b/plansys2_domain_expert/test/pddl/factory_processed.pddl @@ -8,6 +8,9 @@ car - object ) +(:constants +) + (:predicates ( car_assembled ?car0 - car ) ( is_assembly_zone ?zone0 - zone ) @@ -20,6 +23,9 @@ ( robot_available ?robot0 - robot ) ) +(:functions +) + (:durative-action move :parameters ( ?robot0 - robot ?zone1 - zone ?zone2 - zone ) :duration ( = ?duration 5 ) diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h index 3e62ed22d..4a5492807 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Domain.h @@ -215,15 +215,9 @@ class Domain { TokenStruct< std::string > ts = f.parseTypedList( true, types ); - for ( unsigned i = 0; i < ts.size(); ++i ) { - Type * t = getType( ts.types[i] ); - t->constants.insert(ts[i]); - // We need to populate the constants of all supertypes - while(t->supertype != nullptr) { - t = t->supertype; - t->constants.insert(ts[i]); - } - } + for ( unsigned i = 0; i < ts.size(); ++i ) + getType( ts.types[i] )->constants.insert( ts[i] ); + for ( unsigned i = 0; DOMAIN_DEBUG && i < types.size(); ++i ) { std::cout << " "; if ( typed ) std::cout << " " << types[i] << ":"; @@ -543,16 +537,15 @@ class Domain { if ( cons ) { os << "( :constants\n"; - for ( unsigned i = 0; i < types.size(); ++i ) { - for (unsigned j = 0; j < types[i]->constants.size(); j++) { - if (!types[i]->definedInSubtype(types[i]->constants[j])) { - os << "\t" << types[i]->constants[j] << " "; - if ( typed ) - os << "- " << types[i]->name; - os << "\n"; - } + for ( unsigned i = 0; i < types.size(); ++i ) + if ( types[i]->constants.size() ) { + os << "\t"; + for ( unsigned j = 0; j < types[i]->constants.size(); ++j ) + os << types[i]->constants[j] << " "; + if ( typed ) + os << "- " << types[i]->name; + os << "\n"; } - } os << ")\n"; } diff --git a/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h b/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h index 427271d4c..d99885734 100644 --- a/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h +++ b/plansys2_pddl_parser/include/plansys2_pddl_parser/Type.h @@ -51,16 +51,6 @@ class Type { for ( unsigned i = 0; i < t->subtypes.size(); ++i ) subtypes.push_back( ts.get( t->subtypes[i]->name ) ); } - - bool definedInSubtype( const std::string & s) const { - for (unsigned i = 0; i < subtypes.size(); ++i) { - for (unsigned j = 0; j < subtypes[i]->constants.size(); ++j) { - if (subtypes[i]->constants[j] == s) return true; - } - if (subtypes[i]->definedInSubtype(s)) return true; - } - return false; - } void print( std::ostream & stream ) const { stream << name; diff --git a/plansys2_pddl_parser/src/parser.cpp b/plansys2_pddl_parser/src/parser.cpp index 23c8ef07f..b04e5a749 100644 --- a/plansys2_pddl_parser/src/parser.cpp +++ b/plansys2_pddl_parser/src/parser.cpp @@ -25,10 +25,8 @@ int main( int argc, char *argv[] ) { // Read multiagent domain and instance Domain domain( domain_str ); + Instance instance( domain, instance_str ); std::cout << domain << std::endl; - - Instance instance( domain, instance_str ); - std::cout << instance << std::endl; } \ No newline at end of file diff --git a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp index bca9c3f7d..ab0b297d6 100644 --- a/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp +++ b/plansys2_pddl_parser/src/plansys2_pddl_parser/Utils.cpp @@ -23,83 +23,6 @@ namespace parser namespace pddl { -static void printN(std::ostream & s, uint8_t node_type) { - switch(node_type) { - case plansys2_msgs::msg::Node::UNKNOWN: - s << "NODE TYPE UNKNOWN" << std::endl; - break; - case plansys2_msgs::msg::Node::AND: - s << "NODE TYPE AND" << std::endl; - break; - case plansys2_msgs::msg::Node::OR: - s << "NODE TYPE OR" << std::endl; - break; - case plansys2_msgs::msg::Node::NOT: - s << "NODE TYPE NOT" << std::endl; - break; - case plansys2_msgs::msg::Node::PREDICATE: - s << "NODE TYPE PRED" << std::endl; - break; - case plansys2_msgs::msg::Node::FUNCTION: - s << "NODE TYPE FUNC" << std::endl; - break; - case plansys2_msgs::msg::Node::EXPRESSION: - s << "NODE TYPE EXPR" << std::endl; - break; - case plansys2_msgs::msg::Node::NUMBER: - s << "NODE TYPE NUM" << std::endl; - break; - case plansys2_msgs::msg::Node::COMP_GE: - s << "NODE TYPE COMP GE" << std::endl; - break; - case plansys2_msgs::msg::Node::COMP_GT: - s << "NODE TYPE COMP GT" << std::endl; - break; - case plansys2_msgs::msg::Node::COMP_LE: - s << "NODE TYPE COMP LE" << std::endl; - break; - case plansys2_msgs::msg::Node::COMP_LT: - s << "NODE TYPE COMP LT" << std::endl; - break; - case plansys2_msgs::msg::Node::COMP_EQ: - s << "NODE TYPE COMP EQ" << std::endl; - break; - case plansys2_msgs::msg::Node::ARITH_ADD: - s << "NODE TYPE ARITH ADD" << std::endl; - break; - case plansys2_msgs::msg::Node::ARITH_SUB: - s << "NODE TYPE ARITH SUB" << std::endl; - break; - case plansys2_msgs::msg::Node::ARITH_MULT: - s << "NODE TYPE ARITH MULT" << std::endl; - break; - case plansys2_msgs::msg::Node::ARITH_DIV: - s << "NODE TYPE ARITH DIV" << std::endl; - break; - case plansys2_msgs::msg::Node::ASSIGN: - s << "NODE TYPE ASSIGN" << std::endl; - break; - case plansys2_msgs::msg::Node::INCREASE: - s << "NODE TYPE INCREASE" << std::endl; - break; - case plansys2_msgs::msg::Node::DECREASE: - s << "NODE TYPE DECREASE" << std::endl; - break; - case plansys2_msgs::msg::Node::SCALE_UP: - s << "NODE TYPE SCALE UP" << std::endl; - break; - case plansys2_msgs::msg::Node::SCALE_DOWN: - s << "NODE TYPE SCALE DOWN" << std::endl; - break; - case plansys2_msgs::msg::Node::FUNCTION_MODIFIER: - s << "NODE TYPE FUNCTION MODIFIER" << std::endl; - break; - default: - s << "NODE TYPE UNDEFINED" << std::endl; - break; - } -} - std::string getReducedString(const std::string & expr) { std::regex nts_chars("[\n\t]*", std::regex_constants::ECMAScript); @@ -199,73 +122,69 @@ std::tuple getExpr(const std::string & input) std::smatch match; int first = std::numeric_limits::max(); - if (std::regex_search(input, match, std::regex("\\s*>="))) { + if (std::regex_search(input, match, std::regex(">="))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::COMP_GE; } } - if (std::regex_search(input, match, std::regex("\\s*>"))) { + if (std::regex_search(input, match, std::regex(">"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::COMP_GT; } } - if (std::regex_search(input, match, std::regex("\\s*<="))) { + if (std::regex_search(input, match, std::regex("<="))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::COMP_LE; } } - if (std::regex_search(input, match, std::regex("\\s*<"))) { + if (std::regex_search(input, match, std::regex("<"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::COMP_LT; } } - if (std::regex_search(input, match, std::regex("\\s*="))) { + if (std::regex_search(input, match, std::regex("="))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::COMP_EQ; } } - if (std::regex_search(input, match, std::regex("\\s*\\*"))) { + if (std::regex_search(input, match, std::regex("\\*"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::ARITH_MULT; } } - if (std::regex_search(input, match, std::regex("\\s*\\/"))) { + if (std::regex_search(input, match, std::regex("\\/"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::ARITH_DIV; } } - if (std::regex_search(input, match, std::regex("\\s*\\+"))) { + if (std::regex_search(input, match, std::regex("\\+"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::ARITH_ADD; } } - if (std::regex_search(input, match, std::regex("\\s*\\-"))) { + if (std::regex_search(input, match, std::regex("\\-"))) { if (static_cast(match.position()) < first) { first = static_cast(match.position()); expr_type = plansys2_msgs::msg::Node::ARITH_SUB; } } - if (expr_type == plansys2_msgs::msg::Node::UNKNOWN) { - std::cerr << "getExpr: Error parsing expresion [" << input << "]" << std::endl; - } - return std::make_tuple(expr_type, first); } @@ -620,15 +539,11 @@ std::string toStringExpression(const plansys2_msgs::msg::Tree & tree, uint32_t n ret = "(- "; break; default: - ret = "(UNKNOWN "; - std::cerr << "toStringExpression: Unknown expression type: "; - printN(std::cerr, tree.nodes[node_id].expression_type); - std::cerr << std::endl; break; } for (auto child_id : tree.nodes[node_id].children) { - ret += " " + toString(tree, child_id, negate); + ret += toString(tree, child_id, negate); } ret += ")"; @@ -679,10 +594,6 @@ plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, { std::string wexpr = getReducedString(expr); - if (wexpr == "(and)" || wexpr == "(and )") { - return nullptr; - } - auto default_node_type = plansys2_msgs::msg::Node::UNKNOWN; switch (parent) { case plansys2_msgs::msg::Node::AND: @@ -701,9 +612,11 @@ plansys2_msgs::msg::Node::SharedPtr fromString(plansys2_msgs::msg::Tree & tree, default_node_type = plansys2_msgs::msg::Node::FUNCTION; break; } - auto node_type = getNodeType(wexpr, default_node_type); + if (wexpr == "(and)" || wexpr == "(and )") { + return nullptr; + } switch (node_type) { case plansys2_msgs::msg::Node::AND: { auto node = std::make_shared(); diff --git a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp index 80e83d31f..9f6effca2 100644 --- a/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp +++ b/plansys2_problem_expert/src/plansys2_problem_expert/ProblemExpert.cpp @@ -482,15 +482,7 @@ ProblemExpert::isValidPredicate(const plansys2::Predicate & predicate) auto arg_type = getInstance(predicate.parameters[i].name); if (!arg_type.has_value()) { - // It might be a constant, so we check if the type is correct same_types = false; - auto constants = domain_expert_->getConstants(model_predicate.value().parameters[i].type); - for (auto constant : constants) { - if (constant == predicate.parameters[i].name) { - same_types = true; - break; - } - } } else if (arg_type.value().type != model_predicate.value().parameters[i].type) { bool isSubtype = false; for (std::string subType : model_predicate.value().parameters[i].sub_types) { @@ -527,15 +519,7 @@ ProblemExpert::isValidFunction(const plansys2::Function & function) auto arg_type = getInstance(function.parameters[i].name); if (!arg_type.has_value()) { - // It might be a constant, so we check if the type is correct same_types = false; - auto constants = domain_expert_->getConstants(model_function.value().parameters[i].type); - for (auto constant : constants) { - if (constant == function.parameters[i].name) { - same_types = true; - break; - } - } } else if (arg_type.value().type != model_function.value().parameters[i].type) { bool isSubtype = false; for (std::string subType : model_function.value().parameters[i].sub_types) {