robossembler/runtime
8
0
Fork 0
Code Issues 3 Pull requests Projects Releases Packages Wiki Activity Actions

from stash

Browse source
This commit is contained in:
Ilya Uraev 2023-12-05 22:37:28 +03:00
parent d72c06efea
commit 76cd4319eb
8 changed files with 828 additions and 839 deletions
Download patch file Download diff file Expand all files Collapse all files

66
rbs_task_planner/src/task_planner_controller.cpp
View file

@ -13,47 +13,39 @@
#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();
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...");
}
}
}
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_;
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[])
{
int main(int argc, char *argv[]) {
rclcpp::init(argc, argv);
rclcpp::spin(std::make_shared<TaskPlannerController>());
rclcpp::shutdown();