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

⚡ update extra and update pddl terminal sequence

Browse source
This commit is contained in:
Ilya Uraev 2022-01-09 21:39:15 +04:00
parent e1d0bad642
commit 346bf33c9d
2 changed files with 2 additions and 111 deletions
Download patch file Download diff file Expand all files Collapse all files

3
rasms_manipulator/README.md
View file

@ -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
```

110
rasms_manipulator/src/rasmsnode.cpp
View file

@ -1,110 +0,0 @@
#include <memory>
#include <random>
#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<plansys2::DomainExpertClient>();
problem_expert_ = std::make_shared<plansys2::ProblemExpertClient>();
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
planner_client_ = std::make_shared<plansys2::PlannerClient>();
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<plansys2::ProblemExpertClient> problem_expert_;
std::shared_ptr<plansys2::DomainExpertClient> domain_expert_;
std::shared_ptr<plansys2::ExecutorClient> executor_client_;
std::shared_ptr<plansys2::PlannerClient> planner_client_;
};
int main(int argc, char** argv)
{
rclcpp::init(argc, argv);
auto node = std::make_shared<RasmsController>();
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;
}