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

add controller

Browse source
This commit is contained in:
Splinter1984 2022-01-10 01:04:02 +08:00
parent 8c6b912e00
commit 085b3e2d4b
2 changed files with 39 additions and 32 deletions
Download patch file Download diff file Expand all files Collapse all files

2
rasms_manipulator/launch/rasms_manipulation.launch.py
View file

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

59
rasms_manipulator/src/rasms_controller_node.cpp
View file

@ -43,23 +43,6 @@ public:
executor_client_ = std::make_shared<plansys2::ExecutorClient>();
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,21 +119,20 @@ private:
std::shared_ptr<plansys2::PlannerClient> planner_client_;
};
int main(int argc, char **argv)
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())
{
rclcpp::Rate rate(5);
node->step();
rate.sleep();
rclcpp::spin_some(node->get_node_base_interface());
node->step();
}
rate.sleep();
rclcpp::shutdown();
return 0;