Move independent actions above a branch point

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.

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
 {

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<std::string> 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(

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
   "$<BUILD_INTERFACE:${CMAKE_CURRENT_SOURCE_DIR}/include>"

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.

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.

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 <cstddef>
+#include <cstdint>
+#include <memory>
+#include <vector>
+
+#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<std::shared_ptr<ScheduledNode>> 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<ScheduledNode>;
+
+/// 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<std::vector<std::uint32_t>> scheduled_groups(const Schedule & schedule);
+
+}  // namespace plansys2
+
+#endif  // PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_SCHEDULE_HPP_