add task planner runner node

rbs_task_planner/CMakeLists.txt

@@ -30,7 +30,16 @@ set(dependencies
     test_msgs
 )
 
-install(DIRECTORY launch domain problems config DESTINATION share/${PROJECT_NAME})
+install(DIRECTORY launch domain problems config 
+        DESTINATION share/${PROJECT_NAME})
+
+add_executable(execute_plan src/task_planner_controller.cpp)
+ament_target_dependencies(execute_plan ${dependencies})
+
+install(TARGETS execute_plan
+        ARCHIVE DESTINATION lib
+        LIBRARY DESTINATION lib
+        RUNTIME DESTINATION lib/${PROJECT_NAME})
 
 if(BUILD_TESTING)
     find_package(ament_lint_auto REQUIRED)

rbs_task_planner/src/task_planner_controller.cpp

@@ -0,0 +1,61 @@
+#include <plansys2_pddl_parser/Utils.h>
+
+#include <memory>
+
+#include "plansys2_msgs/msg/action_execution_info.hpp"
+#include "plansys2_msgs/msg/plan.hpp"
+
+#include "plansys2_domain_expert/DomainExpertClient.hpp"
+#include "plansys2_executor/ExecutorClient.hpp"
+#include "plansys2_planner/PlannerClient.hpp"
+#include "plansys2_problem_expert/ProblemExpertClient.hpp"
+
+#include "rclcpp/rclcpp.hpp"
+#include "rclcpp_action/rclcpp_action.hpp"
+
+class TaskPlannerController : public rclcpp::Node
+{
+    public:
+    TaskPlannerController() : rclcpp::Node("task_planner_controller") 
+    {
+        domain_expert_ = std::make_shared<plansys2::DomainExpertClient>();
+        planner_client_ = std::make_shared<plansys2::PlannerClient>();
+        problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
+        executor_client_ = std::make_shared<plansys2::ExecutorClient>();
+        run_plan();
+    }
+
+    void run_plan()
+    {
+        auto domain = domain_expert_->getDomain();
+        auto problem = problem_expert_->getProblem();
+        auto plan = planner_client_->getPlan(domain, problem);
+        if(!plan.has_value())
+        {
+            RCLCPP_ERROR(this->get_logger(), "Could not find plan to reach goal %s",
+            parser::pddl::toString(problem_expert_->getGoal()).c_str());
+        }
+        else
+        {
+            if (executor_client_->start_plan_execution(plan.value()))
+            {
+                RCLCPP_INFO(this->get_logger(), "Execute plan...");
+            }
+        }
+    }
+
+    private:
+    std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
+    std::shared_ptr<plansys2::PlannerClient> planner_client_;
+    std::shared_ptr<plansys2::ProblemExpertClient> problem_expert_;
+    std::shared_ptr<plansys2::ExecutorClient> executor_client_;
+
+};
+
+int main(int argc, char * argv[])
+{
+  rclcpp::init(argc, argv);
+  rclcpp::spin(std::make_shared<TaskPlannerController>());
+  rclcpp::shutdown();
+  return 0;
+}