diff --git a/eplansys_docs/demos/two_sites.rst b/eplansys_docs/demos/two_sites.rst index 51b88f3f..63be6c7c 100644 --- a/eplansys_docs/demos/two_sites.rst +++ b/eplansys_docs/demos/two_sites.rst @@ -63,25 +63,28 @@ What the dispatch changes ------------------------- With ``parallel_dispatch`` off, which is the default, the policy is dispatched -one node at a time and the mission takes about twenty-four seconds: eighteen of -declared duration and the rest in dispatch. +one node at a time and the mission takes about twenty-two and a half seconds: +eighteen of declared duration and the rest in dispatch. -With it on, the builder looks for runs of the policy whose actions are -independent --- classically, by asking the domain whether either one's effects -touch what the other requires, and epistemically, by requiring that the agents -they name be disjoint --- and renders each run as one ``Parallel``. It says -which runs it found: +With it on, the builder moves each action as early as the actions it depends on +allow, and dispatches each run of independent actions as one ``Parallel``. Two +actions are independent classically when neither one's effects touch what the +other requires, which the domain answers, and epistemically when the agents they +name are disjoint. It says which runs it found: .. code-block:: text dispatching together: (goto_north north), (goto_south south) - dispatching together: (relay_north north relay), (scan_south south) - -The two drives leave together, and the report from the north site overlaps the -scan of the south one. The mission takes about seventeen seconds. - -Seventeen is not nine, which is what the two halves would take if each ran from -the start. The scan of the north site is where the policy branches, and nothing -below a branch can begin before the branch is resolved, so the south half waits -for a scan it has no stake in. Lifting an independent action across a branch -point is what the pass does not do. + dispatching together: (scan_north north), (scan_south south) + +The south scan is written under both outcomes of the north one, and depends on +neither. Every branch of the north scan begins with it once the north report +has been moved below it, so it moves above the branch point and runs beside the +north scan; the two outcomes then choose the way on, one after the other. The +mission takes about fifteen seconds, from eleven of declared duration: the +drives, the scans, and the two reports. + +Each half takes nine seconds of declared duration on its own. The two together +take eleven because both reports go to ``relay``: two actions that name the +same agent run one after the other, so the south report waits for the north +one. diff --git a/plansys2_epistemic_bt_builder/include/plansys2_epistemic_bt_builder/epistemic_bt_builder.hpp b/plansys2_epistemic_bt_builder/include/plansys2_epistemic_bt_builder/epistemic_bt_builder.hpp index 12492857..ca7e0489 100644 --- a/plansys2_epistemic_bt_builder/include/plansys2_epistemic_bt_builder/epistemic_bt_builder.hpp +++ b/plansys2_epistemic_bt_builder/include/plansys2_epistemic_bt_builder/epistemic_bt_builder.hpp @@ -48,12 +48,13 @@ class DomainExpertClient; * * With `parallel_dispatch` set it does use the domain for one thing. A policy * fixes an order, and where two consecutive nodes had no reason to be ordered - * the mission waits for nothing. The runs where that is so are found by - * `parallel_groups`, whose classical half is answered here, from the domain: - * two actions are independent when neither one's effects touch a predicate the - * other's requirements or effects mention. Each such run is dispatched as one - * `Parallel`. Without the parameter the tree is exactly the one this builder - * rendered before the pass existed. + * the mission waits for nothing. `schedule_policy` moves each action as early + * as the actions it depends on allow, above a branch when every branch begins + * with it, and dispatches each run of independent actions as one `Parallel`. + * Its classical half of independence is answered here, from the domain: two + * actions are independent when neither one's effects touch a predicate the + * other's requirements or effects mention. Without the parameter the tree is + * exactly the one this builder rendered before the pass existed. */ class EpistemicBTBuilder : public BTBuilder { diff --git a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp index 5a461b64..d81a9a8f 100644 --- a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp +++ b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp @@ -28,6 +28,7 @@ #include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_epistemic_executor/policy.hpp" #include "plansys2_epistemic_executor/policy_parallel.hpp" +#include "plansys2_epistemic_executor/policy_schedule.hpp" #include "plansys2_pddl_parser/AmentIndexCompat.hpp" #include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" @@ -221,21 +222,25 @@ std::string EpistemicBTBuilder::get_tree(const plansys2_msgs::msg::Plan & curren return policy_to_bt(policy, bt_action_, precision_); } - const auto groups = parallel_groups( + const auto schedule = schedule_policy( policy, [this](const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b) { return classically_independent(a, b); }); - for (const auto & group : groups) { + // A group copied into several branches is one group to whoever reads this. + std::set reported; + for (const auto & group : scheduled_groups(schedule)) { std::string named; for (const auto member : group) { named += (named.empty() ? "" : ", ") + policy.item(member).action; } - RCLCPP_INFO(logger(), "dispatching together: %s", named.c_str()); + if (reported.insert(named).second) { + RCLCPP_INFO(logger(), "dispatching together: %s", named.c_str()); + } } - return policy_to_bt(policy, bt_action_, precision_, groups); + return policy_to_bt(policy, schedule, bt_action_, precision_); } bool EpistemicBTBuilder::classically_independent( diff --git a/plansys2_epistemic_executor/CMakeLists.txt b/plansys2_epistemic_executor/CMakeLists.txt index 14512e6c..6ca0c299 100644 --- a/plansys2_epistemic_executor/CMakeLists.txt +++ b/plansys2_epistemic_executor/CMakeLists.txt @@ -24,6 +24,7 @@ add_library(${PROJECT_NAME}_policy SHARED src/policy.cpp src/policy_bt.cpp src/policy_parallel.cpp + src/policy_schedule.cpp ) target_include_directories(${PROJECT_NAME}_policy PUBLIC "$" diff --git a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp index 26b84488..860484a5 100644 --- a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp +++ b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp @@ -19,6 +19,7 @@ #include "plansys2_epistemic_executor/policy.hpp" #include "plansys2_epistemic_executor/policy_parallel.hpp" +#include "plansys2_epistemic_executor/policy_schedule.hpp" namespace plansys2 { @@ -122,6 +123,21 @@ std::string policy_to_bt( int precision, const ParallelGroups & groups); +/** + * @brief Render a policy as `schedule_policy` arranged it. + * + * The overload above is this one given the policy as written, with its groups + * marked. A schedule may render one policy node at several places, one per + * branch it was copied into, and a group may hold members that branch: the + * members run together and their outcomes then choose the continuation, one + * switch per member that branches. + */ +std::string policy_to_bt( + const Policy & policy, + const Schedule & schedule, + const std::string & action_bt, + int precision); + /// The action id the executor keys its action map by, for one policy node. /// This has to agree with plansys2::BTBuilder::to_action_id exactly: it is the /// name by which a behavior tree node finds the action it is meant to drive. diff --git a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp index 49658e77..0b8c1a0f 100644 --- a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp +++ b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp @@ -75,6 +75,13 @@ using Independence = */ ParallelGroups parallel_groups(const Policy & policy, const Independence & independent); +/// Whether two items may run at the same time: the test `parallel_groups` +/// applies to every pair it groups, in both of its halves. `independent` may be +/// empty, which leaves the epistemic half alone. +bool may_overlap( + const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b, + const Independence & independent); + /// The agents an item's action names, from the grounded epistemic name and the /// PDDL expression alike. Exposed because it is the whole of the epistemic /// half of the independence test and is worth testing on its own. diff --git a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_schedule.hpp b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_schedule.hpp new file mode 100644 index 00000000..53d480b4 --- /dev/null +++ b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_schedule.hpp @@ -0,0 +1,101 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_SCHEDULE_HPP_ +#define PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_SCHEDULE_HPP_ + +#include +#include +#include +#include + +#include "plansys2_epistemic_executor/policy.hpp" +#include "plansys2_epistemic_executor/policy_parallel.hpp" + +namespace plansys2 +{ + +/** + * @brief One node of a policy arranged for dispatch. + * + * `item` is the policy node whose action is rendered here: its guard, its + * action and its update. `next` is what follows, aligned with the item's + * outcomes when it branches, one entry when it does not, and empty when it ends + * the policy; a null entry is an outcome that completes the policy. + * + * The same item may appear at several places, because moving a node above a + * branch copies what it passed into every branch. Only one of them runs in any + * one execution. + */ +struct ScheduledNode +{ + std::uint32_t item{0}; + std::vector> next; + + /// How many nodes, this one first, start together. A group is followed along + /// `next`, and every branch of a member leads to the same action as the next + /// member: the members are one action each, run once, and their outcomes + /// choose the continuation after all of them have finished. + std::size_t group{1}; +}; + +using Schedule = std::shared_ptr; + +/// The policy exactly as it is written: one node per item, nothing moved and +/// nothing grouped. Rendering it gives the tree `policy_to_bt` always gave. +Schedule policy_schedule(const Policy & policy); + +/// The same, with the groups `parallel_groups` found dispatched together. +Schedule policy_schedule(const Policy & policy, const ParallelGroups & groups); + +/** + * @brief A policy rearranged so that independent actions start as early as + * their dependencies allow. + * + * A policy fixes an order among actions that often had no reason to be + * ordered, and a branch fixes one more: nothing written below a sensing action + * starts before its outcome is known, even an action the outcome has no + * bearing on. In the two-site survey the south scan waits for the north one to + * be read, although it is written under both of its outcomes. + * + * Two things are done about it: + * + * - An action is moved above its parent when the two are independent and the + * action could start earlier. Its earliest start is the end of the latest + * action above it that it depends on, by declared duration. Above a branch + * it may only move when every branch begins with it, and the branch is then + * copied under each of its outcomes. Moving one of two independent actions + * past the other leaves both earliest starts where they were, so the + * rearrangement ends. + * + * - A chain of pairwise independent actions is started together, as + * `parallel_groups` does, except that a member may branch as long as every + * one of its branches continues with the next member. The members' outcomes + * then choose the continuation, one after another, once all have finished. + * + * Independence is `parallel_groups`': classical, as `independent` answers from + * the domain, and epistemic, from the agents the actions name. + * + * A plan with no continuations is PlanSys2's own sequence and is returned as + * written, as `parallel_groups` leaves it too. + */ +Schedule schedule_policy(const Policy & policy, const Independence & independent); + +/// The groups of a schedule, each as the items that start together, in the +/// order a depth-first walk meets them. What a builder reports. +std::vector> scheduled_groups(const Schedule & schedule); + +} // namespace plansys2 + +#endif // PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_SCHEDULE_HPP_ diff --git a/plansys2_epistemic_executor/src/policy_bt.cpp b/plansys2_epistemic_executor/src/policy_bt.cpp index 362bf06c..6532dd9e 100644 --- a/plansys2_epistemic_executor/src/policy_bt.cpp +++ b/plansys2_epistemic_executor/src/policy_bt.cpp @@ -112,123 +112,151 @@ std::string policy_to_bt(const Policy & policy, const std::string & action_bt, i std::string policy_to_bt( const Policy & policy, const std::string & action_bt, int precision, const ParallelGroups & groups) +{ + return policy_to_bt(policy, policy_schedule(policy, groups), action_bt, precision); +} + +std::string policy_to_bt( + const Policy & policy, const Schedule & schedule, const std::string & action_bt, + int precision) { const std::string tmpl = action_bt.empty() ? kDefaultEpistemicActionBT : action_bt; - // Which group a node heads. The rest of a group's members are never rendered - // on their own: the chain that led to them is what the group replaces, and - // they are reached by the head. - std::map heads; - for (const auto & group : groups) { - if (group.size() > 1) { - heads[group.front()] = &group; - } - } + // One node's own block, with whatever is to follow it spliced in. + // + // One blackboard entry per node, named after it: two sensing actions in + // flight never share a key, whether they are on different branches or in the + // same group, and the name says which node wrote it when reading a Groot + // trace. `epistemic_observed_N` is what the robot says it saw and + // `epistemic_outcome_N` is what the epistemic state made of it; they agree + // whenever the report names an outcome the action defines, and keeping them + // apart is what lets the state disagree. + const auto block_of = + [&](std::uint32_t at, const std::string & continuations) -> std::string { + const auto & node = policy.item(at); + const std::string at_id = std::to_string(at); + + std::string block = tmpl; + replace_all(block, "NODE_NAME", escape("node_" + at_id + " " + node.action)); + replace_all(block, "ACTION_ID", escape(policy_action_id(node, precision))); + replace_all(block, "NODE_ID", at_id); + replace_all(block, "OUTCOME_KEY", "epistemic_outcome_" + at_id); + replace_all(block, "OBSERVED_KEY", "epistemic_observed_" + at_id); + replace_all(block, "CONTINUATIONS", shift(continuations, 1)); + return block + "\n"; + }; + + // A switch over what `decided_by` observed, with one child per outcome of + // `at`. They are the same node unless `at` is a copy of a group member, + // which never runs itself: the member it copies ran, and its outcome is the + // one to read. + const auto switch_of = + [&](const ScheduledNode & at, std::uint32_t decided_by, const auto & child) -> std::string { + const auto & node = policy.item(at.item); + + std::string branches; + for (std::size_t i = 0; i < at.next.size(); ++i) { + branches += indent(1) + "\n"; + if (!at.next[i]) { + // The policy is complete on this outcome. Saying so explicitly keeps + // the branch arity equal to the outcome list, so the switch can + // index one by the other. + branches += indent(1) + "\n"; + } else { + branches += shift(child(at.next[i]), 1); + } + } + + std::string outcome_list; + for (std::size_t i = 0; i < node.outcomes.size(); ++i) { + outcome_list += (i ? ";" : "") + node.outcomes[i]; + } + + return + "\n" + + branches + + "\n"; + }; // Each node renders its own subtree and splices its continuations into it, // so the recursion returns a block rather than writing into a shared buffer. - // A node that heads a parallel group renders the whole group instead, with - // the continuation of its last member after the group rather than inside it: - // what follows a group follows all of it. - const auto render = [&](std::uint32_t index, const auto & self) -> std::string { - // One blackboard entry per node, named after it: two sensing actions in - // flight never share a key, whether they are on different branches or in - // the same group, and the name says which node wrote it when reading a - // Groot trace. `epistemic_observed_N` is what the robot says it saw and - // `epistemic_outcome_N` is what the epistemic state made of it; they - // agree whenever the report names an outcome the action defines, and - // keeping them apart is what lets the state disagree. - - // What follows this node once it has run: nothing, the next block, or a + const auto render = [&](const Schedule & at, const auto & self) -> std::string { + // What follows a node once it has run: nothing, the next block, or a // switch over what it observed. - const auto continuations_of = [&](std::uint32_t at) -> std::string { - const auto & node = policy.item(at); - - const auto only = policy.only_successor(at); - if (only) { + const auto continuations_of = + [&](const ScheduledNode & node, std::uint32_t decided_by) -> std::string { + if (node.next.size() == 1) { // Nothing to choose: the continuation simply follows. This is what // makes a classical plan render as the flat sequence PlanSys2 // builds. - return self(*only, self); + return node.next.front() ? self(node.next.front(), self) : ""; } - - if (node.children.size() <= 1) { + if (node.next.empty()) { return ""; // this node ends the policy } - - std::string branches; - for (std::size_t i = 0; i < node.children.size(); ++i) { - branches += indent(1) + "\n"; - if (node.children[i] == plansys2_msgs::msg::PlanItem::POLICY_DONE) { - // The policy is complete on this outcome. Saying so explicitly - // keeps the branch arity equal to the outcome list, so the - // switch can index one by the other. - branches += indent(1) + "\n"; - } else { - branches += shift(self(node.children[i], self), 1); - } - } - - std::string outcome_list; - for (std::size_t i = 0; i < node.outcomes.size(); ++i) { - outcome_list += (i ? ";" : "") + node.outcomes[i]; - } - - return - "\n" + - branches + - "\n"; + return switch_of( + node, decided_by, [&](const Schedule & next) {return self(next, self);}); }; - // One node's own block, with whatever is to follow it spliced in. - const auto block_of = - [&](std::uint32_t at, const std::string & continuations) -> std::string { - const auto & node = policy.item(at); - const std::string at_id = std::to_string(at); - - std::string block = tmpl; - replace_all(block, "NODE_NAME", escape("node_" + at_id + " " + node.action)); - replace_all(block, "ACTION_ID", escape(policy_action_id(node, precision))); - replace_all(block, "NODE_ID", at_id); - replace_all(block, "OUTCOME_KEY", "epistemic_outcome_" + at_id); - replace_all(block, "OBSERVED_KEY", "epistemic_observed_" + at_id); - replace_all(block, "CONTINUATIONS", shift(continuations, 1)); - return block + "\n"; - }; + if (at->group <= 1) { + return block_of(at->item, continuations_of(*at, at->item)); + } - const auto group = heads.find(index); - if (group != heads.end()) { - // The group runs as one, so no member carries a continuation: the - // chain that linked them is what the group replaces, and what follows - // the last of them follows the whole group. - std::string members; - for (const auto member : *group->second) { - members += shift(block_of(member, ""), 1); + // A group runs as one, so no member carries a continuation: the chain + // that linked them is what the group replaces, and what follows the last + // of them follows the whole group. + std::vector members; + const ScheduledNode * member = at.get(); + for (std::size_t i = 0; i < at->group; ++i) { + members.push_back(member->item); + if (i + 1 < at->group) { + member = member->next.front().get(); } + } - // The group and what follows it are one element, not two. A node's - // rendering is spliced in wherever a single child is expected --- a - // switch branch is exactly that --- and two siblings there would give - // the switch more branches than the policy has outcomes. - const std::string after = continuations_of(group->second->back()); - - return - "\n" + - indent(1) + "second->size()) + - "\" failure_count=\"1\">\n" + - shift(members, 1) + - indent(1) + "\n" + - shift(after, 1) + - "\n"; + std::string blocks; + for (const auto item : members) { + blocks += shift(block_of(item, ""), 1); } - return block_of(index, continuations_of(index)); + // Once every member has finished, their outcomes choose the way on, one + // member after another. A member that branches is present as the same + // action on each of its branches, so the walk reaches a copy of the next + // member whichever outcome it takes. + const auto after_member = + [&](const ScheduledNode & node, std::size_t position, const auto & again) -> std::string { + if (position + 1 == members.size()) { + return continuations_of(node, members[position]); + } + if (node.next.size() == 1) { + return again(*node.next.front(), position + 1, again); + } + return switch_of( + node, members[position], + [&](const Schedule & next) {return again(*next, position + 1, again);}); + }; + + // The group and what follows it are one element, not two. A node's + // rendering is spliced in wherever a single child is expected --- a + // switch branch is exactly that --- and two siblings there would give the + // switch more branches than the policy has outcomes. + const std::string after = after_member(*at, 0, after_member); + + return + "item)) + "\">\n" + + indent(1) + "\n" + + shift(blocks, 1) + + indent(1) + "\n" + + shift(after, 1) + + "\n"; }; std::string body; - if (!policy.empty()) { - body = shift(render(Policy::root(), render), 3); + if (!policy.empty() && schedule) { + body = shift(render(schedule, render), 3); } // The goal check is outside the policy rather than at each leaf. A leaf is diff --git a/plansys2_epistemic_executor/src/policy_parallel.cpp b/plansys2_epistemic_executor/src/policy_parallel.cpp index 06c4a8d4..ead7a9af 100644 --- a/plansys2_epistemic_executor/src/policy_parallel.cpp +++ b/plansys2_epistemic_executor/src/policy_parallel.cpp @@ -118,6 +118,13 @@ bool epistemically_independent( } // namespace +bool may_overlap( + const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b, + const Independence & independent) +{ + return epistemically_independent(a, b) && (!independent || independent(a, b)); +} + std::vector item_agents(const plansys2_msgs::msg::PlanItem & item) { std::vector names; @@ -157,11 +164,7 @@ ParallelGroups parallel_groups(const Policy & policy, const Independence & indep const bool joins = std::all_of( group.begin(), group.end(), [&](std::uint32_t member) { - const auto & held = policy.item(member); - if (!epistemically_independent(held, candidate)) { - return false; - } - return !independent || independent(held, candidate); + return may_overlap(policy.item(member), candidate, independent); }); if (!joins) { diff --git a/plansys2_epistemic_executor/src/policy_schedule.cpp b/plansys2_epistemic_executor/src/policy_schedule.cpp new file mode 100644 index 00000000..dc92c6cd --- /dev/null +++ b/plansys2_epistemic_executor/src/policy_schedule.cpp @@ -0,0 +1,317 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "plansys2_epistemic_executor/policy_schedule.hpp" + +#include +#include +#include +#include +#include +#include +#include + +namespace plansys2 +{ + +namespace +{ + +using Node = ScheduledNode; + +Schedule written( + const Policy & policy, std::uint32_t index, std::map & made) +{ + const auto found = made.find(index); + if (found != made.end()) { + return found->second; + } + + auto node = std::make_shared(); + node->item = index; + made[index] = node; + + // The three readings policy_to_bt has always made of a node: one + // continuation, several chosen by outcome, or none. + const auto & item = policy.item(index); + if (const auto only = policy.only_successor(index)) { + node->next = {written(policy, *only, made)}; + } else if (item.children.size() > 1) { + for (const auto child : item.children) { + node->next.push_back( + child == plansys2_msgs::msg::PlanItem::POLICY_DONE ? + nullptr : written(policy, child, made)); + } + } + return node; +} + +/// Past this many nodes the rearrangement is abandoned. Moving a node above a +/// branch copies what it passed once per outcome, and a policy that branches +/// on many independent observations would multiply without bound; one that +/// does is dispatched as `parallel_groups` would dispatch it. +constexpr std::size_t kMaxNodes = 20000; + +class Scheduler +{ +public: + Scheduler(const Policy & policy, const Independence & independent) + : policy_(policy), independent_(independent) {} + + /// An action's earliest start below `above`, and what it depends on there. + using Above = std::vector>; + + /// The subtree with every action moved as high as it may go. + Schedule settle(const Schedule & node, const Above & above) + { + if (!node || too_big()) { + return node; + } + + const double start = earliest(node->item, above); + auto copy = fresh(*node); + + auto below = above; + below.emplace_back(node->item, start); + for (auto & next : copy->next) { + next = settle(next, below); + } + + if (auto lifted = lift(copy, above, start)) { + return lifted; + } + return copy; + } + + /// Mark the chains that start together, heads only. + void group(const Schedule & node) + { + if (!node || !seen_.insert(node.get()).second) { + return; + } + + std::vector members{node->item}; + std::vector frontier{node.get()}; + + while (true) { + std::vector following; + bool whole = true; + for (const auto * at : frontier) { + if (at->next.empty()) { + whole = false; + break; + } + for (const auto & next : at->next) { + if (!next) { + whole = false; + break; + } + following.push_back(next.get()); + } + } + if (!whole || following.empty()) { + break; + } + + const auto candidate = following.front()->item; + const bool twins = std::all_of( + following.begin(), following.end(), + [&](const Node * other) {return same_action(other->item, candidate);}); + const bool free = std::all_of( + members.begin(), members.end(), + [&](std::uint32_t member) {return overlap(member, candidate);}); + if (!twins || !free) { + break; + } + + members.push_back(candidate); + frontier = std::move(following); + } + + node->group = members.size(); + for (auto * at : frontier) { + for (const auto & next : at->next) { + group(next); + } + } + } + + bool too_big() const {return made_ > kMaxNodes;} + +private: + Schedule fresh(const Node & from) + { + ++made_; + return std::make_shared(from); + } + + bool same_action(std::uint32_t a, std::uint32_t b) const + { + const auto & first = policy_.item(a); + const auto & second = policy_.item(b); + return first.action == second.action && + first.epistemic_action == second.epistemic_action && + first.outcomes == second.outcomes; + } + + bool overlap(std::uint32_t a, std::uint32_t b) + { + const auto key = std::minmax(a, b); + const auto found = overlap_.find(key); + if (found != overlap_.end()) { + return found->second; + } + const bool answer = may_overlap(policy_.item(a), policy_.item(b), independent_); + overlap_[key] = answer; + return answer; + } + + /// When the action could start at the soonest: once everything above it + /// that it depends on has finished, by declared duration. + double earliest(std::uint32_t item, const Above & above) + { + double start = 0.0; + for (const auto & [ancestor, ancestor_start] : above) { + if (!overlap(ancestor, item)) { + const auto took = std::max(0.0, static_cast(policy_.item(ancestor).duration)); + start = std::max(start, ancestor_start + took); + } + } + return start; + } + + /// Move what follows `parent` above it, when that is the same action on + /// every branch, independent of the parent, and able to start sooner. The + /// parent is copied under each of the moved action's own continuations, + /// with what used to follow the moved action there below it. + Schedule lift(const Schedule & parent, const Above & above, double parent_start) + { + if (parent->next.empty()) { + return nullptr; + } + for (const auto & next : parent->next) { + if (!next) { + return nullptr; // a branch that ends has nothing to move + } + } + + const auto & first = parent->next.front(); + for (const auto & next : parent->next) { + if (!same_action(next->item, first->item)) { + return nullptr; + } + } + if (!overlap(parent->item, first->item)) { + return nullptr; + } + + const double start = earliest(first->item, above); + if (!(start < parent_start)) { + return nullptr; + } + + // A moved action that ended the policy leaves one continuation, the + // parent, whose branches then end where the moved action did. + const std::size_t slots = std::max(first->next.size(), 1); + + auto moved = fresh(*first); + moved->next.clear(); + auto below = above; + below.emplace_back(first->item, start); + + for (std::size_t slot = 0; slot < slots; ++slot) { + auto copy = fresh(*parent); + for (std::size_t branch = 0; branch < copy->next.size(); ++branch) { + const auto & twin = parent->next[branch]; + copy->next[branch] = twin->next.empty() ? nullptr : twin->next[slot]; + } + moved->next.push_back(settle(copy, below)); + } + return moved; + } + + const Policy & policy_; + const Independence & independent_; + std::map, bool> overlap_; + std::set seen_; + std::size_t made_{0}; +}; + +} // namespace + +Schedule policy_schedule(const Policy & policy) +{ + return policy_schedule(policy, ParallelGroups{}); +} + +Schedule policy_schedule(const Policy & policy, const ParallelGroups & groups) +{ + if (policy.empty()) { + return nullptr; + } + + std::map made; + auto root = written(policy, Policy::root(), made); + + for (const auto & group : groups) { + const auto head = made.find(group.front()); + if (group.size() > 1 && head != made.end()) { + head->second->group = group.size(); + } + } + return root; +} + +Schedule schedule_policy(const Policy & policy, const Independence & independent) +{ + if (policy.empty() || policy.sequential()) { + return policy_schedule(policy); + } + + Scheduler scheduler(policy, independent); + auto root = scheduler.settle(policy_schedule(policy), {}); + if (scheduler.too_big()) { + return policy_schedule(policy, parallel_groups(policy, independent)); + } + scheduler.group(root); + return root; +} + +std::vector> scheduled_groups(const Schedule & schedule) +{ + std::vector> groups; + std::set seen; + + const auto walk = [&](const Schedule & node, const auto & self) -> void { + if (!node || !seen.insert(node.get()).second) { + return; + } + if (node->group > 1) { + std::vector members; + const ScheduledNode * at = node.get(); + for (std::size_t i = 0; i < node->group && at; ++i) { + members.push_back(at->item); + at = i + 1 < node->group ? at->next.front().get() : nullptr; + } + groups.push_back(members); + } + for (const auto & next : node->next) { + self(next, self); + } + }; + walk(schedule, walk); + return groups; +} + +} // namespace plansys2 diff --git a/plansys2_epistemic_executor/test/CMakeLists.txt b/plansys2_epistemic_executor/test/CMakeLists.txt index b58d3eee..2d0c5cb1 100644 --- a/plansys2_epistemic_executor/test/CMakeLists.txt +++ b/plansys2_epistemic_executor/test/CMakeLists.txt @@ -7,6 +7,9 @@ target_link_libraries(policy_bt_test ${PROJECT_NAME}_policy behaviortree_cpp::be ament_add_gtest(policy_parallel_test policy_parallel_test.cpp TIMEOUT 120) target_link_libraries(policy_parallel_test ${PROJECT_NAME}_policy) +ament_add_gtest(policy_schedule_test policy_schedule_test.cpp TIMEOUT 120) +target_link_libraries(policy_schedule_test ${PROJECT_NAME}_policy behaviortree_cpp::behaviortree_cpp) + # Loads the node library the way the executor does, by path, and checks a # rendered tree instantiates against the real nodes rather than stubs. ament_add_gtest(bt_plugin_test bt_plugin_test.cpp TIMEOUT 120) diff --git a/plansys2_epistemic_executor/test/policy_schedule_test.cpp b/plansys2_epistemic_executor/test/policy_schedule_test.cpp new file mode 100644 index 00000000..891da400 --- /dev/null +++ b/plansys2_epistemic_executor/test/policy_schedule_test.cpp @@ -0,0 +1,310 @@ +// Copyright 2026 Intelligent Robotics Lab +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include +#include +#include +#include + +#include "behaviortree_cpp/bt_factory.h" +#include "behaviortree_cpp/control_node.h" + +#include "plansys2_epistemic_executor/policy_bt.hpp" +#include "plansys2_epistemic_executor/policy_schedule.hpp" + +using plansys2::Policy; +using plansys2::Schedule; +using plansys2::policy_to_bt; +using plansys2::schedule_policy; +using plansys2::scheduled_groups; +using PlanItem = plansys2_msgs::msg::PlanItem; + +namespace +{ + +constexpr auto kDone = PlanItem::POLICY_DONE; + +PlanItem action( + const std::string & expression, const std::string & grounded, float duration, + const std::vector & children = {}, + const std::vector & outcomes = {}) +{ + PlanItem item; + item.action = expression; + item.epistemic_action = grounded; + item.duration = duration; + item.children = children; + item.outcomes = outcomes; + item.sensing = children.size() > 1; + return item; +} + +/// The two-site survey as the planner returns it, with the demo's durations: +/// both drives, the north scan, and under each of its outcomes the north report +/// followed by the south scan and the south report. +plansys2_msgs::msg::Plan two_site_policy() +{ + plansys2_msgs::msg::Plan plan; + plan.epistemic_goal = "(and (Kw north dirty-north) (Kw south dirty-south))"; + const std::vector north = {"e-scan-north-dirty", "e-scan-north-clean"}; + const std::vector south = {"e-scan-south-dirty", "e-scan-south-clean"}; + plan.items = { + action("(goto_north north)", "goto-north_north", 4.0f, {1}), + action("(goto_south south)", "goto-south_south", 4.0f, {2}), + action("(scan_north north)", "scan-north_north", 3.0f, {3, 7}, north), + action("(relay_north north relay)", "relay-north-dirty_north_relay", 2.0f, {4}), + action("(scan_south south)", "scan-south_south", 3.0f, {5, 6}, south), + action("(relay_south south relay)", "relay-south-dirty_south_relay", 2.0f, {}), + action("(relay_south south relay)", "relay-south-clean_south_relay", 2.0f, {}), + action("(relay_north north relay)", "relay-north-clean_north_relay", 2.0f, {8}), + action("(scan_south south)", "scan-south_south", 3.0f, {9, 10}, south), + action("(relay_south south relay)", "relay-south-dirty_south_relay", 2.0f, {}), + action("(relay_south south relay)", "relay-south-clean_south_relay", 2.0f, {}), + }; + return plan; +} + +const plansys2::Independence kAlways = + [](const PlanItem &, const PlanItem &) {return true;}; + +const plansys2::Independence kNever = + [](const PlanItem &, const PlanItem &) {return false;}; + +/// What one execution runs, given what each sensing action observes: the +/// actions of the policy as written, in order. +std::vector run_policy( + const Policy & policy, const std::map & observed) +{ + std::vector ran; + std::uint32_t at = Policy::root(); + while (true) { + const auto & item = policy.item(at); + ran.push_back(item.epistemic_action); + if (const auto only = policy.only_successor(at)) { + at = *only; + continue; + } + if (item.children.size() <= 1) { + return ran; + } + const auto next = item.children[observed.at(item.epistemic_action)]; + if (next == kDone) { + return ran; + } + at = next; + } +} + +/// The same execution through a schedule, walked the way its rendering runs: +/// a group's members all run, then their outcomes choose the way on. Each entry +/// is the set of actions started together. +std::vector> run_schedule( + const Policy & policy, const Schedule & schedule, + const std::map & observed) +{ + std::vector> ran; + const plansys2::ScheduledNode * at = schedule.get(); + while (at) { + std::vector together; + const plansys2::ScheduledNode * member = at; + for (std::size_t i = 0; i < at->group; ++i) { + together.push_back(policy.item(member->item).epistemic_action); + if (i + 1 == at->group) { + break; + } + // Every branch of a member continues with the next one, so which branch + // the walk takes does not matter until the last member. + const auto & item = policy.item(member->item); + member = member->next.size() == 1 ? member->next.front().get() : + member->next[observed.at(item.epistemic_action)].get(); + } + ran.push_back(together); + + if (member->next.empty()) { + break; + } + const auto & item = policy.item(member->item); + at = member->next.size() == 1 ? member->next.front().get() : + member->next[observed.at(item.epistemic_action)].get(); + } + return ran; +} + +std::size_t count_of(const std::string & haystack, const std::string & needle) +{ + std::size_t count = 0; + for (std::size_t at = haystack.find(needle); at != std::string::npos; + at = haystack.find(needle, at + needle.size())) + { + ++count; + } + return count; +} + +class SwitchStub : public BT::ControlNode +{ +public: + SwitchStub(const std::string & name, const BT::NodeConfig & conf) + : BT::ControlNode(name, conf) {} + + BT::NodeStatus tick() override {return BT::NodeStatus::SUCCESS;} + + static BT::PortsList providedPorts() + { + return { + BT::InputPort("node"), + BT::InputPort("outcome"), + BT::InputPort("outcomes")}; + } +}; + +void expect_parses(const std::string & xml) +{ + BT::BehaviorTreeFactory factory; + const auto ok = [](BT::TreeNode &) {return BT::NodeStatus::SUCCESS;}; + factory.registerSimpleCondition( + "CheckKnowledge", ok, + {BT::InputPort("node"), BT::InputPort("action")}); + factory.registerSimpleCondition( + "CheckEpistemicGoal", ok, {BT::InputPort("goal")}); + factory.registerSimpleAction( + "ApplyEpistemicUpdate", ok, + {BT::InputPort("node"), BT::InputPort("action"), + BT::InputPort("observed"), BT::OutputPort("outcome")}); + factory.registerSimpleCondition( + "CheckBeliefUnchanged", ok, + {BT::InputPort("action"), BT::InputPort("enabled")}); + for (const auto & name : {"WaitAtStartReq", "CheckOverAllReq", "CheckAtEndReq"}) { + factory.registerSimpleCondition(name, ok, {BT::InputPort("action")}); + } + for (const auto & name : {"ApplyAtStartEffect", "ApplyAtEndEffect"}) { + factory.registerSimpleAction(name, ok, {BT::InputPort("action")}); + } + factory.registerSimpleAction( + "ExecuteAction", ok, + {BT::InputPort("action"), BT::OutputPort("outcome")}); + factory.registerNodeType("EpistemicSwitch"); + + EXPECT_NO_THROW( + { + auto tree = factory.createTreeFromText(xml); + (void)tree; + }) << xml; +} + +} // namespace + +TEST(PolicySchedule, the_south_scan_runs_with_the_north_one) +{ + // The south scan is written under both outcomes of the north one, and + // nothing about it depends on them. Moving it up puts it beside the north + // scan, and the drives were already beside each other. + const Policy policy(two_site_policy()); + const auto schedule = schedule_policy(policy, kAlways); + + const auto groups = scheduled_groups(schedule); + ASSERT_EQ(groups.size(), 2u); + EXPECT_EQ(groups[0], (std::vector{0, 1})); + EXPECT_EQ(groups[1], (std::vector{2, 4})); +} + +TEST(PolicySchedule, every_execution_runs_what_the_policy_would) +{ + // For each combination of what the two sites hold, the schedule runs the + // same actions the policy does, and never one before an action it depends + // on: each report after its own site's scan, and the two reports to the + // relay one after the other. + const Policy policy(two_site_policy()); + const auto schedule = schedule_policy(policy, kAlways); + + for (const std::size_t north : {0u, 1u}) { + for (const std::size_t south : {0u, 1u}) { + SCOPED_TRACE("north " + std::to_string(north) + ", south " + std::to_string(south)); + const std::map observed = { + {"scan-north_north", north}, {"scan-south_south", south}}; + + auto written = run_policy(policy, observed); + const auto steps = run_schedule(policy, schedule, observed); + + std::vector scheduled; + for (const auto & step : steps) { + scheduled.insert(scheduled.end(), step.begin(), step.end()); + } + std::sort(written.begin(), written.end()); + std::sort(scheduled.begin(), scheduled.end()); + EXPECT_EQ(scheduled, written); + + // Drives, scans, then the reports one at a time. + ASSERT_EQ(steps.size(), 4u); + EXPECT_EQ(steps[0].size(), 2u); + EXPECT_EQ(steps[1].size(), 2u); + EXPECT_EQ(steps[2].front().rfind("relay-north-", 0), 0u); + EXPECT_EQ(steps[3].front().rfind("relay-south-", 0), 0u); + } + } +} + +TEST(PolicySchedule, the_moved_scan_runs_once_and_both_branches_read_it) +{ + const Policy policy(two_site_policy()); + const auto xml = policy_to_bt(policy, schedule_policy(policy, kAlways), "", 3); + + EXPECT_EQ(count_of(xml, "