diff --git a/rasms_manipulator/README.md b/rasms_manipulator/README.md index 6dbb508..23d452c 100644 --- a/rasms_manipulator/README.md +++ b/rasms_manipulator/README.md @@ -41,8 +41,9 @@ Then into plansys2_terminal paste command (see updates into pddl/commands) ```bash set instance rasms robot set instance one zone +set instance two zone set predicate (robot_at rasms one) -set goal (and(robot_moved rasms one)) +set goal (and(robot_at rasms two)) run ``` diff --git a/rasms_manipulator/src/rasmsnode.cpp b/rasms_manipulator/src/rasmsnode.cpp deleted file mode 100644 index 24a8a81..0000000 --- a/rasms_manipulator/src/rasmsnode.cpp +++ /dev/null @@ -1,110 +0,0 @@ -#include -#include - -#include "plansys2_msgs/msg/action_execution_info.hpp" - -#include "plansys2_executor/ExecutorClient.hpp" -#include "plansys2_domain_expert/DomainExpertClient.hpp" -#include "plansys2_problem_expert/ProblemExpertClient.hpp" -#include "plansys2_planner/PlannerClient.hpp" - -#include "rclcpp/rclcpp.hpp" -#include "rclcpp_action/rclcpp_action.hpp" - -class RasmsController: public rclcpp::Node -{ -public: - RasmsController(): rclcpp::Node("rasms_controller") - {} - - void init() - { - domain_expert_ = std::make_shared(); - problem_expert_ = std::make_shared(); - executor_client_ = std::make_shared(); - planner_client_ = std::make_shared(); - - init_knowledge(); - - auto domain = domain_expert_->getDomain(); - auto problem = problem_expert_->getProblem(); - auto plan = planner_client_->getPlan(domain, problem); - - if (plan.has_value()) - { - if (!executor_client_->start_plan_execution(plan.value())) - { - RCLCPP_ERROR(get_logger(), "Error starting a new plan (first)"); - } - } else { - RCLCPP_ERROR_STREAM( - this->get_logger(), "Could not find plan to reach goal " << - parser::pddl::toString(problem_expert_->getGoal())); - } - } - - void init_knowledge() - { - problem_expert_->addInstance(plansys2::Instance{"rasms", "robot"}); - problem_expert_->setGoal(plansys2::Goal("robot moved")); - } - - std::string status_to_string(int8_t status) - { - switch (status) - { - case plansys2_msgs::msg::ActionExecutionInfo::NOT_EXECUTED: - return "NOT_EXECUTED"; - break; - case plansys2_msgs::msg::ActionExecutionInfo::EXECUTING: - return "EXECUTING"; - break; - case plansys2_msgs::msg::ActionExecutionInfo::FAILED: - return "FAILED"; - break; - case plansys2_msgs::msg::ActionExecutionInfo::SUCCEEDED: - return "SUCCEEDED"; - break; - case plansys2_msgs::msg::ActionExecutionInfo::CANCELLED: - return "CANCELLED"; - break; - default: - return "UNKNOWN"; - break; - } - } - - void step() - { - if (!executor_client_->execute_and_check_plan()) - { - RCLCPP_INFO(get_logger(), "Done!"); - } - } - -private: - std::shared_ptr problem_expert_; - std::shared_ptr domain_expert_; - std::shared_ptr executor_client_; - std::shared_ptr planner_client_; -}; - -int main(int argc, char** argv) -{ - rclcpp::init(argc, argv); - auto node = std::make_shared(); - - node->init(); - - rclcpp::Rate rate(1); - while (rclcpp::ok()) - { - rate.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); - node->step(); - } - - rclcpp::shutdown(); - - return 0; -} \ No newline at end of file