Skip to content

Commit

Permalink
Bump Behaviortree.CPP v3 to v4 - fix executor test
Browse files Browse the repository at this point in the history
  • Loading branch information
@adfea625
adfea625 committed Feb 25, 2024
1 parent f7e03c9 commit 03adabf
Show file tree
Hide file tree
Showing 3 changed files with 166 additions and 166 deletions.
3 changes: 2 additions & 1 deletion plansys2_executor/include/plansys2_executor/behavior_tree/check_timeout_node.hpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -49,7 +49,8 @@ class CheckTimeout : public BT::ActionNodeBase
}

private:
std::chrono::high_resolution_clock::time_point start_;
rclcpp_lifecycle::LifecycleNode::SharedPtr node_;
rclcpp::Time start_;
std::shared_ptr<std::map<std::string, ActionExecutionInfo>> action_map_;
std::shared_ptr<plansys2::ProblemExpertClient> problem_client_;
};
Expand Down
21 changes: 10 additions & 11 deletions plansys2_executor/src/plansys2_executor/behavior_tree/check_timeout_node.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -36,6 +36,9 @@ CheckTimeout::CheckTimeout(
problem_client_ =
config().blackboard->get<std::shared_ptr<plansys2::ProblemExpertClient>>(
"problem_client");

node_ = config().blackboard->get<rclcpp_lifecycle::LifecycleNode::SharedPtr>("node");
start_ = node_->now();
}

BT::NodeStatus
Expand All @@ -44,22 +47,18 @@ CheckTimeout::tick()
std::string action;
getInput("action", action);

if (status() == BT::NodeStatus::IDLE) {
start_ = std::chrono::high_resolution_clock::now();
}
setStatus(BT::NodeStatus::RUNNING);

if ((*action_map_)[action].action_executor != nullptr) {
double duration = (*action_map_)[action].duration;
double duration_overrun_percentage = (*action_map_)[action].duration_overrun_percentage;
if (duration_overrun_percentage >= 0) {
double max_duration = (1.0 + duration_overrun_percentage / 100.0) * duration;
auto current_time = std::chrono::high_resolution_clock::now();
auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
current_time - start_);
if (elapsed_time > std::chrono::duration<double>(max_duration)) {
std::cerr << "Actual duration of " << action << " exceeds max duration (" << std::fixed <<
std::setprecision(2) << max_duration << " secs)." << std::endl;
auto current_time = node_->now();
auto elapsed_time = (current_time - start_).seconds();
if (elapsed_time > max_duration) {
RCLCPP_ERROR_STREAM(
node_->get_logger(),
"Actual duration of " << action << " exceeds max duration (" << std::fixed <<
std::setprecision(2) << max_duration << " secs).");
return BT::NodeStatus::FAILURE;
}
}
Expand Down
308 changes: 154 additions & 154 deletions plansys2_executor/test/unit/executor_test.cpp
View file
Original file line number Diff line number Diff line change
Expand Up @@ -107,7 +107,7 @@ class MoveAction : public plansys2::ActionExecutorClient
{
std::cerr << "MoveAction::on_activate" << std::endl;
counter_ = 0;
start_ = std::chrono::high_resolution_clock::now();
start_ = now();

return ActionExecutorClient::on_activate(state);
}
Expand All @@ -120,17 +120,18 @@ class MoveAction : public plansys2::ActionExecutorClient
}

cycles_++;
auto current_time = std::chrono::high_resolution_clock::now();
auto elapsed_time = std::chrono::duration_cast<std::chrono::milliseconds>(
current_time - start_);
auto current_time = now();
auto elapsed_time = (current_time - start_).seconds();

rclcpp::Rate rate(100);

if (runtime_ > 1e-5) {
if (elapsed_time > std::chrono::duration<double>(runtime_)) {
if (elapsed_time > runtime_) {
finish(true, 1.0, "completed");
executions_++;
} else {
send_feedback((static_cast<double>(elapsed_time.count()) / 1000.0) / runtime_, "running");
std::this_thread::sleep_for(std::chrono::milliseconds(10));
send_feedback(elapsed_time / runtime_, "running");
rate.sleep();
}
} else {
if (counter_++ > 3) {
Expand All @@ -146,7 +147,7 @@ class MoveAction : public plansys2::ActionExecutorClient
int executions_;
int cycles_;
double runtime_;
std::chrono::high_resolution_clock::time_point start_;
rclcpp::Time start_;
};

class TransportAction : public plansys2::ActionExecutorClient
Expand Down Expand Up @@ -1550,153 +1551,152 @@ TEST(executor, executor_client_cancel_plan)
t.join();
}

TEST(executor, action_timeout)
{
auto test_node_1 = rclcpp::Node::make_shared("test_node_1");
auto test_node_2 = rclcpp::Node::make_shared("test_node_2");
auto test_node_3 = rclcpp::Node::make_shared("test_node_3");
auto test_node_4 = rclcpp::Node::make_shared("test_node_4");
auto test_lf_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lf_node");
auto domain_node = std::make_shared<plansys2::DomainExpertNode>();
auto problem_node = std::make_shared<plansys2::ProblemExpertNode>();
auto planner_node = std::make_shared<plansys2::PlannerNode>();
auto executor_node = std::make_shared<ExecutorNodeTest>();

auto move_action_node = MoveAction::make_shared("move_action_performer", 100ms);
move_action_node->set_parameter({"action_name", "move"});
move_action_node->set_runtime(10.0);

auto domain_client = std::make_shared<plansys2::DomainExpertClient>();
auto problem_client = std::make_shared<plansys2::ProblemExpertClient>();
auto planner_client = std::make_shared<plansys2::PlannerClient>();
auto executor_client = std::make_shared<plansys2::ExecutorClient>();

std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor");

domain_node->set_parameter({"model_file", pkgpath + "/pddl/factory3.pddl"});
problem_node->set_parameter({"model_file", pkgpath + "/pddl/factory3.pddl"});
executor_node->set_parameter(
{"default_action_bt_xml_filename",
pkgpath + "/test_behavior_trees/test_action_timeout_bt.xml"});
executor_node->set_parameter({"bt_builder_plugin", "SimpleBTBuilder"});
executor_node->set_parameter({"action_timeouts.actions", std::vector<std::string>({"move"})});
// have to declare because the actions vector above was not available at node creation
executor_node->declare_parameter<double>(
"action_timeouts.move.duration_overrun_percentage", 1.0);
executor_node->set_parameter({"action_timeouts.move.duration_overrun_percentage", 1.0});

rclcpp::executors::SingleThreadedExecutor exe;

exe.add_node(domain_node->get_node_base_interface());
exe.add_node(problem_node->get_node_base_interface());
exe.add_node(planner_node->get_node_base_interface());
exe.add_node(executor_node->get_node_base_interface());
exe.add_node(move_action_node->get_node_base_interface());
exe.add_node(test_lf_node->get_node_base_interface());

bool finish = false;
std::thread t([&]() {
while (!finish) {exe.spin_some();}
});

domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
move_action_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

// TEST(executor, action_timeout)
// {
// auto test_node_1 = rclcpp::Node::make_shared("test_node_1");
// auto test_node_2 = rclcpp::Node::make_shared("test_node_2");
// auto test_node_3 = rclcpp::Node::make_shared("test_node_3");
// auto test_node_4 = rclcpp::Node::make_shared("test_node_4");
// auto test_lf_node = rclcpp_lifecycle::LifecycleNode::make_shared("test_lf_node");
// auto domain_node = std::make_shared<plansys2::DomainExpertNode>();
// auto problem_node = std::make_shared<plansys2::ProblemExpertNode>();
// auto planner_node = std::make_shared<plansys2::PlannerNode>();
// auto executor_node = std::make_shared<ExecutorNodeTest>();

// auto move_action_node = MoveAction::make_shared("move_action_performer", 100ms);
// move_action_node->set_parameter({"action_name", "move"});
// move_action_node->set_runtime(10.0);

// auto domain_client = std::make_shared<plansys2::DomainExpertClient>();
// auto problem_client = std::make_shared<plansys2::ProblemExpertClient>();
// auto planner_client = std::make_shared<plansys2::PlannerClient>();
// auto executor_client = std::make_shared<plansys2::ExecutorClient>();

// std::string pkgpath = ament_index_cpp::get_package_share_directory("plansys2_executor");

// domain_node->set_parameter({"model_file", pkgpath + "/pddl/factory3.pddl"});
// problem_node->set_parameter({"model_file", pkgpath + "/pddl/factory3.pddl"});
// executor_node->set_parameter(
// {"default_action_bt_xml_filename",
// pkgpath + "/test_behavior_trees/test_action_timeout_bt.xml"});
// executor_node->set_parameter({"bt_builder_plugin", "SimpleBTBuilder"});
// executor_node->set_parameter({"action_timeouts.actions", std::vector<std::string>({"move"})});
// // have to declare because the actions vector above was not available at node creation
// executor_node->declare_parameter<double>(
// "action_timeouts.move.duration_overrun_percentage", 1.0);
// executor_node->set_parameter({"action_timeouts.move.duration_overrun_percentage", 1.0});

// rclcpp::executors::SingleThreadedExecutor exe;

// exe.add_node(domain_node->get_node_base_interface());
// exe.add_node(problem_node->get_node_base_interface());
// exe.add_node(planner_node->get_node_base_interface());
// exe.add_node(executor_node->get_node_base_interface());
// exe.add_node(move_action_node->get_node_base_interface());
// exe.add_node(test_lf_node->get_node_base_interface());

// bool finish = false;
// std::thread t([&]() {
// while (!finish) {exe.spin_some();}
// });

// domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
// problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
// planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
// move_action_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
// test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);
// executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE);

// {
// rclcpp::Rate rate(10);
// auto start = test_node_1->now();
// while ((test_node_1->now() - start).seconds() < 0.5) {
// rate.sleep();
// }
// }

// domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
// problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
// planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
// executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
// test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);

// {
// rclcpp::Rate rate(10);
// auto start = test_node_1->now();
// while ((test_node_1->now() - start).seconds() < 0.5) {
// rate.sleep();
// }
// }

// ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("r2d2", "robot")));
// ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone")));
// ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("steering_wheels_zone", "zone")));
// ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("body_car_zone", "zone")));
// ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone")));

// std::vector<std::string> predicates = {
// "(robot_at r2d2 steering_wheels_zone)",
// "(robot_available r2d2)",
// "(battery_full r2d2)",
// };

// for (const auto & pred : predicates) {
// ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred)));
// }

// problem_client->setGoal(plansys2::Goal("(and(robot_at r2d2 assembly_zone))"));

// auto domain = domain_client->getDomain();
// auto problem = problem_client->getProblem();
// auto plan = planner_client->getPlan(domain, problem);

// ASSERT_FALSE(domain.empty());
// ASSERT_FALSE(problem.empty());
// ASSERT_TRUE(plan.has_value());

// {
// rclcpp::Rate rate(10);

// ASSERT_TRUE(executor_client->start_plan_execution(plan.value()));

// while (rclcpp::ok() && executor_client->execute_and_check_plan()) {
// auto feedback = executor_client->getFeedBack();
// std::stringstream ss;
// ss.setf(std::ios_base::fixed, std::ios_base::floatfield);
// for (const auto & action_feedback : feedback.action_execution_status) {
// ss << "[" << action_feedback.action << " " << std::setprecision(1) <<
// action_feedback.completion * 100.0 <<
// "%]";
// }
// auto & clk = *executor_node->get_clock();
// RCLCPP_WARN_THROTTLE(executor_node->get_logger(), clk, 500, "%s", ss.str().c_str());
// rate.sleep();
// }
// }

// ASSERT_TRUE(executor_client->getResult().has_value());
// auto result = executor_client->getResult().value();
// ASSERT_FALSE(result.success);
// ASSERT_EQ(
// result.action_execution_status[0].status,
// plansys2_msgs::msg::ActionExecutionInfo::CANCELLED);

// {
// rclcpp::Rate rate(10);
// auto start = test_node_1->now();
// while ((test_node_1->now() - start).seconds() < 2.0) {
// rate.sleep();
// }
// }

// ASSERT_EQ(
// move_action_node->get_current_state().id(),
// lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

// finish = true;
// t.join();
// }
{
rclcpp::Rate rate(10);
auto start = test_node_1->now();
while ((test_node_1->now() - start).seconds() < 0.5) {
rate.sleep();
}
}

domain_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
problem_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
planner_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
executor_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);
test_lf_node->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_ACTIVATE);

{
rclcpp::Rate rate(10);
auto start = test_node_1->now();
while ((test_node_1->now() - start).seconds() < 0.5) {
rate.sleep();
}
}

ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("r2d2", "robot")));
ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("wheels_zone", "zone")));
ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("steering_wheels_zone", "zone")));
ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("body_car_zone", "zone")));
ASSERT_TRUE(problem_client->addInstance(plansys2::Instance("assembly_zone", "zone")));

std::vector<std::string> predicates = {
"(robot_at r2d2 steering_wheels_zone)",
"(robot_available r2d2)",
"(battery_full r2d2)",
};

for (const auto & pred : predicates) {
ASSERT_TRUE(problem_client->addPredicate(plansys2::Predicate(pred)));
}

problem_client->setGoal(plansys2::Goal("(and(robot_at r2d2 assembly_zone))"));

auto domain = domain_client->getDomain();
auto problem = problem_client->getProblem();
auto plan = planner_client->getPlan(domain, problem);

ASSERT_FALSE(domain.empty());
ASSERT_FALSE(problem.empty());
ASSERT_TRUE(plan.has_value());

{
rclcpp::Rate rate(10);

ASSERT_TRUE(executor_client->start_plan_execution(plan.value()));

while (rclcpp::ok() && executor_client->execute_and_check_plan()) {
auto feedback = executor_client->getFeedBack();
std::stringstream ss;
ss.setf(std::ios_base::fixed, std::ios_base::floatfield);
for (const auto & action_feedback : feedback.action_execution_status) {
ss << "[" << action_feedback.action << " " << std::setprecision(1) <<
action_feedback.completion * 100.0 <<
"%]";
}
auto & clk = *executor_node->get_clock();
RCLCPP_WARN_THROTTLE(executor_node->get_logger(), clk, 500, "%s", ss.str().c_str());
rate.sleep();
}
}

ASSERT_TRUE(executor_client->getResult().has_value());
auto result = executor_client->getResult().value();
ASSERT_FALSE(result.success);
ASSERT_EQ(
result.action_execution_status[0].status,
plansys2_msgs::msg::ActionExecutionInfo::CANCELLED);

{
rclcpp::Rate rate(10);
auto start = test_node_1->now();
while ((test_node_1->now() - start).seconds() < 2.0) {
rate.sleep();
}
}

ASSERT_EQ(
move_action_node->get_current_state().id(),
lifecycle_msgs::msg::State::PRIMARY_STATE_INACTIVE);

finish = true;
t.join();
}


int main(int argc, char ** argv)
Expand Down

0 comments on commit 03adabf

Please sign in to comment.