diff --git a/rasms_manipulator/launch/rasms_manipulation.launch.py b/rasms_manipulator/launch/rasms_manipulation.launch.py index 74a0e9e..7920535 100644 --- a/rasms_manipulator/launch/rasms_manipulation.launch.py +++ b/rasms_manipulator/launch/rasms_manipulation.launch.py @@ -103,6 +103,6 @@ def generate_launch_description(): # Declare the launch options ld.add_action(plansys2_cmd) ld.add_action(move_1) - # ld.add_action(transport_1_cmd) + ld.add_action(transport_1_cmd) return ld \ No newline at end of file diff --git a/rasms_manipulator/src/rasms_controller_node.cpp b/rasms_manipulator/src/rasms_controller_node.cpp index ea9d5f4..694b44b 100644 --- a/rasms_manipulator/src/rasms_controller_node.cpp +++ b/rasms_manipulator/src/rasms_controller_node.cpp @@ -43,23 +43,6 @@ public: executor_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() @@ -68,7 +51,6 @@ public: problem_expert_->addInstance(plansys2::Instance{"one", "zone"}); problem_expert_->addInstance(plansys2::Instance{"two", "zone"}); problem_expert_->addPredicate(plansys2::Predicate("(robot_at rasms one)")); - problem_expert_->setGoal(plansys2::Goal("(and(robot_at rasms one two))")); } std::string status_to_string(int8_t status) @@ -98,9 +80,35 @@ public: void step() { - if (!executor_client_->execute_and_check_plan()) + problem_expert_->setGoal(plansys2::Goal("(and(robot_at rasms two))")); + + auto domain = domain_expert_->getDomain(); + auto problem = problem_expert_->getProblem(); + auto plan = planner_client_->getPlan(domain, problem); + + if (!plan.has_value()) { + std::cout << "Could not find plan to reach goal " << + parser::pddl::toString(problem_expert_->getGoal()) << std::endl; + } + + executor_client_->start_plan_execution(plan.value()); + + auto feedback = executor_client_->getFeedBack(); + + for (const auto & action_feedback : feedback.action_execution_status) { - RCLCPP_INFO(get_logger(), "Done!"); + std::cout << "[" << action_feedback.action << " " << + action_feedback.completion * 100.0 << "%]"; + } + + std::cout << std::endl; + + if (!executor_client_->execute_and_check_plan() && executor_client_->getResult()) + { + if (executor_client_->getResult().value().success) + { + std::cout << "Successful finished " << std::endl; + } } } @@ -111,22 +119,21 @@ private: std::shared_ptr planner_client_; }; -int main(int argc, char **argv) +int main(int argc, char ** argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - + node->init(); + + rclcpp::Rate rate(5); + node->step(); - rclcpp::Rate rate(1); - while (rclcpp::ok()) - { - rate.sleep(); - rclcpp::spin_some(node->get_node_base_interface()); - node->step(); - } - + rate.sleep(); + rclcpp::spin_some(node->get_node_base_interface()); + + rate.sleep(); rclcpp::shutdown(); - + return 0; } \ No newline at end of file