From fb9f29b82f754b5213f0347d008ff8e1afec85c5 Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 12:19:03 -0600 Subject: [PATCH 1/7] A two-site survey, whose halves share nothing The site survey twice over, in different hands. Each scout carries the instrument its own site reads, so neither half of the mission can be done by the other's robot, and no action of one half appears in a precondition of the other. The point of it is the makespan. Eighteen seconds of declared duration are spent one action at a time whatever the policy permits, and this is the mission where that is visible: two halves that were never ordered, dispatched as though they were. --- eplansys_demo/CMakeLists.txt | 19 + .../epddl/survey-sites-problem.epddl | 68 +++ eplansys_demo/epddl/survey-sites.epddl | 297 ++++++++++++++ eplansys_demo/launch/survey_sites_launch.py | 140 +++++++ eplansys_demo/params/survey_sites.yaml | 37 ++ eplansys_demo/pddl/survey-sites-mapping.json | 386 ++++++++++++++++++ eplansys_demo/pddl/survey-sites.pddl | 104 +++++ eplansys_demo/src/survey_sites_actions.cpp | 141 +++++++ eplansys_demo/src/survey_sites_mission.cpp | 117 ++++++ 9 files changed, 1309 insertions(+) create mode 100644 eplansys_demo/epddl/survey-sites-problem.epddl create mode 100644 eplansys_demo/epddl/survey-sites.epddl create mode 100644 eplansys_demo/launch/survey_sites_launch.py create mode 100644 eplansys_demo/params/survey_sites.yaml create mode 100644 eplansys_demo/pddl/survey-sites-mapping.json create mode 100644 eplansys_demo/pddl/survey-sites.pddl create mode 100644 eplansys_demo/src/survey_sites_actions.cpp create mode 100644 eplansys_demo/src/survey_sites_mission.cpp diff --git a/eplansys_demo/CMakeLists.txt b/eplansys_demo/CMakeLists.txt index d4d6c822..97fe18a5 100644 --- a/eplansys_demo/CMakeLists.txt +++ b/eplansys_demo/CMakeLists.txt @@ -50,7 +50,26 @@ target_link_libraries(survey_mission ) target_compile_definitions(survey_mission PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") +add_executable(survey_sites_actions src/survey_sites_actions.cpp) +target_link_libraries(survey_sites_actions + plansys2_executor::plansys2_executor + rclcpp::rclcpp +) +target_compile_definitions(survey_sites_actions PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + +add_executable(survey_sites_mission src/survey_sites_mission.cpp) +target_link_libraries(survey_sites_mission + plansys2_executor::plansys2_executor + plansys2_planner::plansys2_planner + plansys2_problem_expert::plansys2_problem_expert + plansys2_domain_expert::plansys2_domain_expert + ${plansys2_msgs_TARGETS} + rclcpp::rclcpp +) +target_compile_definitions(survey_sites_mission PUBLIC "PLUGINLIB__DISABLE_BOOST_FUNCTIONS") + install(TARGETS corridor_actions corridor_mission survey_actions survey_mission + survey_sites_actions survey_sites_mission RUNTIME DESTINATION lib/${PROJECT_NAME} ) diff --git a/eplansys_demo/epddl/survey-sites-problem.epddl b/eplansys_demo/epddl/survey-sites-problem.epddl new file mode 100644 index 00000000..2993a398 --- /dev/null +++ b/eplansys_demo/epddl/survey-sites-problem.epddl @@ -0,0 +1,68 @@ +(define (problem two-site-survey) + + ;; Four robots at the depot. `north` and `south` are the two scouts, `relay` + ;; is the team member the findings have to reach, and `observer` is the + ;; machine the team does not control and the mission excludes. + ;; + ;; Neither site's state is given. Two atoms are left open, so the model + ;; designates four worlds and the policy branches twice. + ;; + ;; Each scout carries the instrument its own site needs, and only the site's + ;; instrument reads it. That is what keeps the two halves in different hands: + ;; without it the cheapest policy sends one robot to both sites, which is a + ;; correct answer to the mission and not the one this problem is posed to + ;; produce. + + (:domain survey-sites) + + (:requirements + :equality :list-comprehensions + :finitary-S5-theories :modal-goals :knowing-whether + :multi-pointed-models :negative-preconditions + ) + + (:agents north south relay observer) + + (:init + (:and + ;; Who carries what, settled and common knowledge. Every agent is named + ;; for every instrument: an atom the initial state leaves out is an atom + ;; the model has to be uncertain about, and eight such atoms would + ;; designate two hundred and fifty-six worlds to say what four say here. + ([C. All] (instrument-north north)) + ([C. All] (not (instrument-north south))) + ([C. All] (not (instrument-north relay))) + ([C. All] (not (instrument-north observer))) + ([C. All] (instrument-south south)) + ([C. All] (not (instrument-south north))) + ([C. All] (not (instrument-south relay))) + ([C. All] (not (instrument-south observer))) + (:forall (?i - agent) + ([C. All] (not (on-north ?i)))) + (:forall (?i - agent) + ([C. All] (not (on-south ?i)))) + (:forall (?i - agent) + ([C. All] ( (contaminated-north)))) + (:forall (?i - agent) + ([C. All] ( (contaminated-south)))) + ) + ) + + ;; Six conjuncts, three per site, and the two triples share nothing. Each + ;; site has to be visited, its finding has to reach `relay` over a channel + ;; the observer cannot hear, and the observer has to end ignorant of it. + ;; + ;; The two halves are what this problem is for. They are independent in the + ;; model --- no action of one appears in a precondition of the other --- so + ;; any serial policy for the mission is twice as long as the work requires. + (:goal + (and + ([Kw. north] (contaminated-north)) + ([Kw. relay] (contaminated-north)) + ( (contaminated-north)) + ([Kw. south] (contaminated-south)) + ([Kw. relay] (contaminated-south)) + ( (contaminated-south)) + ) + ) +) diff --git a/eplansys_demo/epddl/survey-sites.epddl b/eplansys_demo/epddl/survey-sites.epddl new file mode 100644 index 00000000..2f1181c9 --- /dev/null +++ b/eplansys_demo/epddl/survey-sites.epddl @@ -0,0 +1,297 @@ +(define (domain survey-sites) + + ;; Two sites, and a team large enough to survey both at once. + ;; + ;; This is `survey-team` with the site made plural. Everything that domain + ;; says about audiences holds here unchanged: a scanning robot learns the + ;; answer, the others learn only that it scanned, the open channel reaches + ;; the observer and the team link does not. + ;; + ;; What the second site adds is independence. Nothing a robot does at the + ;; north site bears on the south one: the two findings are separate atoms, + ;; the two drives are separate effects, and no action at one site appears in + ;; any precondition at the other. A policy for this mission therefore holds + ;; two halves that could run at the same time and, dispatched a node at a + ;; time, do not. + ;; + ;; EPDDL parameters are agents, so a site cannot be one. The two halves are + ;; written out instead of quantified, which is verbose and exact. + + (:requirements + :equality :partial-observability :list-comprehensions :lists + :ontic-actions :group-modalities + :negative-preconditions :modal-preconditions :knowing-whether + ) + + (:action-type-libraries intermediate) + + (:predicates + (contaminated-north) ; the north site is contaminated + (contaminated-south) ; the south site is contaminated + (on-north ?i - agent) ; ?i is close enough to scan the north site + (on-south ?i - agent) ; ?i is close enough to scan the south site + (instrument-north ?i - agent); ?i carries the instrument the north site needs + (instrument-south ?i - agent); ?i carries the instrument the south site needs + ) + + (:event nil) + + ;--------------------DRIVE TO A SITE------------------ + ; + ; Ontic and public, as in the one-site mission: the team tracks its members + ; and the observer watches them move. + + (:event e-goto-north + :parameters (?i - agent) + :precondition (not (on-north ?i)) + :effects (on-north ?i) + ) + + (:action goto-north + :parameters (?i - agent) + :action-type (public-ontic (e-goto-north ?i)) + :observability-conditions (default Fully) + ) + + (:event e-goto-south + :parameters (?i - agent) + :precondition (not (on-south ?i)) + :effects (on-south ?i) + ) + + (:action goto-south + :parameters (?i - agent) + :action-type (public-ontic (e-goto-south ?i)) + :observability-conditions (default Fully) + ) + + ;--------------------SCAN A SITE------------------ + ; + ; Semi-private sensing. The scanning robot learns whether that site is + ; contaminated; everyone else learns that it scanned. + + (:event e-scan-north-dirty + :parameters (?i - agent) + :precondition + (and + (instrument-north ?i) + (on-north ?i) + (contaminated-north) + ( (contaminated-north)) ) + ) + + (:event e-scan-north-clean + :parameters (?i - agent) + :precondition + (and + (instrument-north ?i) + (on-north ?i) + (not (contaminated-north)) + ( (contaminated-north)) ) + ) + + (:action scan-north + :parameters (?i - agent) + :action-type (semi-private-sensing + (e-scan-north-dirty ?i) + (e-scan-north-clean ?i) ) + :observability-conditions + (:and + (?i Fully) + (default Partially) ) + ) + + (:event e-scan-south-dirty + :parameters (?i - agent) + :precondition + (and + (instrument-south ?i) + (on-south ?i) + (contaminated-south) + ( (contaminated-south)) ) + ) + + (:event e-scan-south-clean + :parameters (?i - agent) + :precondition + (and + (instrument-south ?i) + (on-south ?i) + (not (contaminated-south)) + ( (contaminated-south)) ) + ) + + (:action scan-south + :parameters (?i - agent) + :action-type (semi-private-sensing + (e-scan-south-dirty ?i) + (e-scan-south-clean ?i) ) + :observability-conditions + (:and + (?i Fully) + (default Partially) ) + ) + + ;--------------------REPORT ON THE OPEN CHANNEL------------------ + ; + ; The trap, once per site: it informs the team in one step and hands the + ; finding to the observer, which the goal forbids. + + (:event e-open-north-dirty + :parameters (?i - agent) + :precondition ([?i] (contaminated-north)) + ) + + (:event e-not-open-north-dirty + :parameters (?i - agent) + :precondition (not ([?i] (contaminated-north))) + ) + + (:action broadcast-north-dirty + :parameters (?i - agent) + :action-type (public-announcement + (e-open-north-dirty ?i) + (e-not-open-north-dirty ?i) ) + :observability-conditions (default Fully) + ) + + (:event e-open-north-clean + :parameters (?i - agent) + :precondition ([?i] (not (contaminated-north))) + ) + + (:event e-not-open-north-clean + :parameters (?i - agent) + :precondition (not ([?i] (not (contaminated-north)))) + ) + + (:action broadcast-north-clean + :parameters (?i - agent) + :action-type (public-announcement + (e-open-north-clean ?i) + (e-not-open-north-clean ?i) ) + :observability-conditions (default Fully) + ) + + (:event e-open-south-dirty + :parameters (?i - agent) + :precondition ([?i] (contaminated-south)) + ) + + (:event e-not-open-south-dirty + :parameters (?i - agent) + :precondition (not ([?i] (contaminated-south))) + ) + + (:action broadcast-south-dirty + :parameters (?i - agent) + :action-type (public-announcement + (e-open-south-dirty ?i) + (e-not-open-south-dirty ?i) ) + :observability-conditions (default Fully) + ) + + (:event e-open-south-clean + :parameters (?i - agent) + :precondition ([?i] (not (contaminated-south))) + ) + + (:event e-not-open-south-clean + :parameters (?i - agent) + :precondition (not ([?i] (not (contaminated-south)))) + ) + + (:action broadcast-south-clean + :parameters (?i - agent) + :action-type (public-announcement + (e-open-south-clean ?i) + (e-not-open-south-clean ?i) ) + :observability-conditions (default Fully) + ) + + ;--------------------REPORT ON THE TEAM CHANNEL------------------ + ; + ; The action the mission needs, once per site and per polarity: the speaker + ; and the named listener learn, and nobody else learns that anything was + ; said. + + (:event e-team-north-dirty + :parameters (?i - agent) + :precondition + (and + (on-north ?i) + ([?i] (contaminated-north)) ) + ) + + (:action relay-north-dirty + :parameters (?i ?j - agent) + :action-type (private-announcement + (e-team-north-dirty ?i) + (nil) ) + :observability-conditions + (:and + (?i Fully) + (?j Fully) + (default Oblivious) ) + ) + + (:event e-team-north-clean + :parameters (?i - agent) + :precondition + (and + (on-north ?i) + ([?i] (not (contaminated-north))) ) + ) + + (:action relay-north-clean + :parameters (?i ?j - agent) + :action-type (private-announcement + (e-team-north-clean ?i) + (nil) ) + :observability-conditions + (:and + (?i Fully) + (?j Fully) + (default Oblivious) ) + ) + + (:event e-team-south-dirty + :parameters (?i - agent) + :precondition + (and + (on-south ?i) + ([?i] (contaminated-south)) ) + ) + + (:action relay-south-dirty + :parameters (?i ?j - agent) + :action-type (private-announcement + (e-team-south-dirty ?i) + (nil) ) + :observability-conditions + (:and + (?i Fully) + (?j Fully) + (default Oblivious) ) + ) + + (:event e-team-south-clean + :parameters (?i - agent) + :precondition + (and + (on-south ?i) + ([?i] (not (contaminated-south))) ) + ) + + (:action relay-south-clean + :parameters (?i ?j - agent) + :action-type (private-announcement + (e-team-south-clean ?i) + (nil) ) + :observability-conditions + (:and + (?i Fully) + (?j Fully) + (default Oblivious) ) + ) +) diff --git a/eplansys_demo/launch/survey_sites_launch.py b/eplansys_demo/launch/survey_sites_launch.py new file mode 100644 index 00000000..0a32bea4 --- /dev/null +++ b/eplansys_demo/launch/survey_sites_launch.py @@ -0,0 +1,140 @@ +# 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. + +""" +The two-site survey: two scouts, two sites and nothing shared between them. + + ros2 launch eplansys_demo survey_sites_launch.py + ros2 launch eplansys_demo survey_sites_launch.py north:=clean south:=dirty + +Stated in EPDDL and ground by plank at start up, so `plank` has to be built and +on PATH. + +The mission is the one-site survey run twice in different hands. Each scout +carries the instrument its own site needs, so neither half can be done by the +other's robot, and no action of one half appears in any precondition of the +other. The policy that comes back therefore holds two independent halves, and +the wall clock the mission prints says whether they were dispatched as such. + + ros2 launch eplansys_demo survey_sites_launch.py parallel:=true + +turns the dispatch of those runs on. It is the same policy either way: what +changes is how much of it is in flight at once, and the elapsed time the +mission prints at the end. +""" + +import os +import tempfile + +from ament_index_python.packages import get_package_share_directory + +from launch import LaunchDescription +from launch.actions import ( + DeclareLaunchArgument, + EmitEvent, + IncludeLaunchDescription, + OpaqueFunction, + RegisterEventHandler, +) +from launch.event_handlers import OnProcessExit +from launch.events import Shutdown +from launch.launch_description_sources import PythonLaunchDescriptionSource +from launch.substitutions import LaunchConfiguration +from launch_ros.actions import Node + + +OUTCOMES = { + 'north': {'dirty': 'e-scan-north-dirty', 'clean': 'e-scan-north-clean'}, + 'south': {'dirty': 'e-scan-south-dirty', 'clean': 'e-scan-south-clean'}, +} + + +def launch_setup(context, *args, **kwargs): + pkg = get_package_share_directory('eplansys_demo') + + domain = os.path.join(pkg, 'epddl', 'survey-sites.epddl') + problem = os.path.join(pkg, 'epddl', 'survey-sites-problem.epddl') + mapping = os.path.join(pkg, 'pddl', 'survey-sites-mapping.json') + model = os.path.join(pkg, 'pddl', 'survey-sites.pddl') + + found = {} + for site in OUTCOMES: + choice = LaunchConfiguration(site).perform(context) + if choice not in OUTCOMES[site]: + raise RuntimeError( + f'{site}:={choice} is not one of {sorted(OUTCOMES[site])}. It is ' + 'what that site turns out to hold.') + found[site] = OUTCOMES[site][choice] + + parallel = LaunchConfiguration('parallel').perform(context).lower() + if parallel not in ('true', 'false'): + raise RuntimeError( + f'parallel:={parallel} is neither true nor false. It says whether the ' + 'two halves of the policy are dispatched together.') + + with open(os.path.join(pkg, 'params', 'survey_sites.yaml')) as handle: + params = handle.read() + params = (params + .replace('EPDDL_DOMAIN', domain) + .replace('EPDDL_PROBLEM', problem) + .replace('MAPPING_FILE', mapping) + .replace('PARALLEL_DISPATCH', parallel)) + + filled = tempfile.NamedTemporaryFile( + mode='w', suffix='_survey_sites.yaml', delete=False) + filled.write(params) + filled.close() + + plansys2 = IncludeLaunchDescription( + PythonLaunchDescriptionSource(os.path.join( + get_package_share_directory('plansys2_bringup'), + 'launch', 'plansys2_bringup_launch_monolithic.py')), + launch_arguments={ + 'model_file': model, + 'params_file': filled.name, + 'epistemic_state': 'True', + }.items()) + + actions = Node( + package='eplansys_demo', + executable='survey_sites_actions', + name='survey_sites_actions', + output='screen', + arguments=['--north', found['north'], '--south', found['south']]) + + mission = Node( + package='eplansys_demo', + executable='survey_sites_mission', + name='survey_sites_mission', + output='screen') + + finish = RegisterEventHandler( + OnProcessExit(target_action=mission, on_exit=[EmitEvent(event=Shutdown())])) + + return [plansys2, actions, mission, finish] + + +def generate_launch_description(): + return LaunchDescription([ + DeclareLaunchArgument( + 'north', default_value='dirty', + description='What the north site turns out to hold: dirty or clean.'), + DeclareLaunchArgument( + 'south', default_value='clean', + description='What the south site turns out to hold: dirty or clean.'), + DeclareLaunchArgument( + 'parallel', default_value='false', + description='Dispatch the policy\'s independent runs together.'), + OpaqueFunction(function=launch_setup), + ]) diff --git a/eplansys_demo/params/survey_sites.yaml b/eplansys_demo/params/survey_sites.yaml new file mode 100644 index 00000000..8599bc16 --- /dev/null +++ b/eplansys_demo/params/survey_sites.yaml @@ -0,0 +1,37 @@ +# The two-site survey, as parameters. +# +# The solver is given EPDDL sources, not a grounded task: it runs plank on +# first use and again whenever the sources change. This is the ordinary way to +# state an epistemic problem, and the counterpart of classical PlanSys2's +# model_file. + +planner: + ros__parameters: + plan_solver_plugins: ["EPISTEMIC"] + EPISTEMIC: + plugin: "plansys2/EpistemicPlanSolver" + epddl_domain: "EPDDL_DOMAIN" + epddl_problem: "EPDDL_PROBLEM" + action_mapping: "MAPPING_FILE" + strategy: "aostar" + conditional_plan: "policy" + +executor: + ros__parameters: + bt_builder_plugin: "EpistemicBTBuilder" + bt_node_plugins: ["libplansys2_epistemic_bt_nodes.so"] + +epistemic_state: + ros__parameters: + # The same sources the planner is given. Both ground them independently; + # neither grounds for the other, and pointing them at one pair is what + # keeps the policy and the model it is checked against in one vocabulary. + epddl_domain: "EPDDL_DOMAIN" + epddl_problem: "EPDDL_PROBLEM" + +# Whether the independent halves of the policy are dispatched together. The +# builder reads this from a node of its own name, which is why it is here and +# not under `executor`. +epistemic_bt_builder: + ros__parameters: + parallel_dispatch: PARALLEL_DISPATCH diff --git a/eplansys_demo/pddl/survey-sites-mapping.json b/eplansys_demo/pddl/survey-sites-mapping.json new file mode 100644 index 00000000..ef8b57ae --- /dev/null +++ b/eplansys_demo/pddl/survey-sites-mapping.json @@ -0,0 +1,386 @@ +{ + "broadcast-north-clean_north": { + "action": "(broadcast_north north)", + "duration": 2.0 + }, + "broadcast-north-clean_observer": { + "action": "(broadcast_north observer)", + "duration": 2.0 + }, + "broadcast-north-clean_relay": { + "action": "(broadcast_north relay)", + "duration": 2.0 + }, + "broadcast-north-clean_south": { + "action": "(broadcast_north south)", + "duration": 2.0 + }, + "broadcast-north-dirty_north": { + "action": "(broadcast_north north)", + "duration": 2.0 + }, + "broadcast-north-dirty_observer": { + "action": "(broadcast_north observer)", + "duration": 2.0 + }, + "broadcast-north-dirty_relay": { + "action": "(broadcast_north relay)", + "duration": 2.0 + }, + "broadcast-north-dirty_south": { + "action": "(broadcast_north south)", + "duration": 2.0 + }, + "broadcast-south-clean_north": { + "action": "(broadcast_south north)", + "duration": 2.0 + }, + "broadcast-south-clean_observer": { + "action": "(broadcast_south observer)", + "duration": 2.0 + }, + "broadcast-south-clean_relay": { + "action": "(broadcast_south relay)", + "duration": 2.0 + }, + "broadcast-south-clean_south": { + "action": "(broadcast_south south)", + "duration": 2.0 + }, + "broadcast-south-dirty_north": { + "action": "(broadcast_south north)", + "duration": 2.0 + }, + "broadcast-south-dirty_observer": { + "action": "(broadcast_south observer)", + "duration": 2.0 + }, + "broadcast-south-dirty_relay": { + "action": "(broadcast_south relay)", + "duration": 2.0 + }, + "broadcast-south-dirty_south": { + "action": "(broadcast_south south)", + "duration": 2.0 + }, + "goto-north_north": { + "action": "(goto_north north)", + "duration": 4.0 + }, + "goto-north_observer": { + "action": "(goto_north observer)", + "duration": 4.0 + }, + "goto-north_relay": { + "action": "(goto_north relay)", + "duration": 4.0 + }, + "goto-north_south": { + "action": "(goto_north south)", + "duration": 4.0 + }, + "goto-south_north": { + "action": "(goto_south north)", + "duration": 4.0 + }, + "goto-south_observer": { + "action": "(goto_south observer)", + "duration": 4.0 + }, + "goto-south_relay": { + "action": "(goto_south relay)", + "duration": 4.0 + }, + "goto-south_south": { + "action": "(goto_south south)", + "duration": 4.0 + }, + "relay-north-clean_north_north": { + "action": "(relay_north north north)", + "duration": 2.0 + }, + "relay-north-clean_north_observer": { + "action": "(relay_north north observer)", + "duration": 2.0 + }, + "relay-north-clean_north_relay": { + "action": "(relay_north north relay)", + "duration": 2.0 + }, + "relay-north-clean_north_south": { + "action": "(relay_north north south)", + "duration": 2.0 + }, + "relay-north-clean_observer_north": { + "action": "(relay_north observer north)", + "duration": 2.0 + }, + "relay-north-clean_observer_observer": { + "action": "(relay_north observer observer)", + "duration": 2.0 + }, + "relay-north-clean_observer_relay": { + "action": "(relay_north observer relay)", + "duration": 2.0 + }, + "relay-north-clean_observer_south": { + "action": "(relay_north observer south)", + "duration": 2.0 + }, + "relay-north-clean_relay_north": { + "action": "(relay_north relay north)", + "duration": 2.0 + }, + "relay-north-clean_relay_observer": { + "action": "(relay_north relay observer)", + "duration": 2.0 + }, + "relay-north-clean_relay_relay": { + "action": "(relay_north relay relay)", + "duration": 2.0 + }, + "relay-north-clean_relay_south": { + "action": "(relay_north relay south)", + "duration": 2.0 + }, + "relay-north-clean_south_north": { + "action": "(relay_north south north)", + "duration": 2.0 + }, + "relay-north-clean_south_observer": { + "action": "(relay_north south observer)", + "duration": 2.0 + }, + "relay-north-clean_south_relay": { + "action": "(relay_north south relay)", + "duration": 2.0 + }, + "relay-north-clean_south_south": { + "action": "(relay_north south south)", + "duration": 2.0 + }, + "relay-north-dirty_north_north": { + "action": "(relay_north north north)", + "duration": 2.0 + }, + "relay-north-dirty_north_observer": { + "action": "(relay_north north observer)", + "duration": 2.0 + }, + "relay-north-dirty_north_relay": { + "action": "(relay_north north relay)", + "duration": 2.0 + }, + "relay-north-dirty_north_south": { + "action": "(relay_north north south)", + "duration": 2.0 + }, + "relay-north-dirty_observer_north": { + "action": "(relay_north observer north)", + "duration": 2.0 + }, + "relay-north-dirty_observer_observer": { + "action": "(relay_north observer observer)", + "duration": 2.0 + }, + "relay-north-dirty_observer_relay": { + "action": "(relay_north observer relay)", + "duration": 2.0 + }, + "relay-north-dirty_observer_south": { + "action": "(relay_north observer south)", + "duration": 2.0 + }, + "relay-north-dirty_relay_north": { + "action": "(relay_north relay north)", + "duration": 2.0 + }, + "relay-north-dirty_relay_observer": { + "action": "(relay_north relay observer)", + "duration": 2.0 + }, + "relay-north-dirty_relay_relay": { + "action": "(relay_north relay relay)", + "duration": 2.0 + }, + "relay-north-dirty_relay_south": { + "action": "(relay_north relay south)", + "duration": 2.0 + }, + "relay-north-dirty_south_north": { + "action": "(relay_north south north)", + "duration": 2.0 + }, + "relay-north-dirty_south_observer": { + "action": "(relay_north south observer)", + "duration": 2.0 + }, + "relay-north-dirty_south_relay": { + "action": "(relay_north south relay)", + "duration": 2.0 + }, + "relay-north-dirty_south_south": { + "action": "(relay_north south south)", + "duration": 2.0 + }, + "relay-south-clean_north_north": { + "action": "(relay_south north north)", + "duration": 2.0 + }, + "relay-south-clean_north_observer": { + "action": "(relay_south north observer)", + "duration": 2.0 + }, + "relay-south-clean_north_relay": { + "action": "(relay_south north relay)", + "duration": 2.0 + }, + "relay-south-clean_north_south": { + "action": "(relay_south north south)", + "duration": 2.0 + }, + "relay-south-clean_observer_north": { + "action": "(relay_south observer north)", + "duration": 2.0 + }, + "relay-south-clean_observer_observer": { + "action": "(relay_south observer observer)", + "duration": 2.0 + }, + "relay-south-clean_observer_relay": { + "action": "(relay_south observer relay)", + "duration": 2.0 + }, + "relay-south-clean_observer_south": { + "action": "(relay_south observer south)", + "duration": 2.0 + }, + "relay-south-clean_relay_north": { + "action": "(relay_south relay north)", + "duration": 2.0 + }, + "relay-south-clean_relay_observer": { + "action": "(relay_south relay observer)", + "duration": 2.0 + }, + "relay-south-clean_relay_relay": { + "action": "(relay_south relay relay)", + "duration": 2.0 + }, + "relay-south-clean_relay_south": { + "action": "(relay_south relay south)", + "duration": 2.0 + }, + "relay-south-clean_south_north": { + "action": "(relay_south south north)", + "duration": 2.0 + }, + "relay-south-clean_south_observer": { + "action": "(relay_south south observer)", + "duration": 2.0 + }, + "relay-south-clean_south_relay": { + "action": "(relay_south south relay)", + "duration": 2.0 + }, + "relay-south-clean_south_south": { + "action": "(relay_south south south)", + "duration": 2.0 + }, + "relay-south-dirty_north_north": { + "action": "(relay_south north north)", + "duration": 2.0 + }, + "relay-south-dirty_north_observer": { + "action": "(relay_south north observer)", + "duration": 2.0 + }, + "relay-south-dirty_north_relay": { + "action": "(relay_south north relay)", + "duration": 2.0 + }, + "relay-south-dirty_north_south": { + "action": "(relay_south north south)", + "duration": 2.0 + }, + "relay-south-dirty_observer_north": { + "action": "(relay_south observer north)", + "duration": 2.0 + }, + "relay-south-dirty_observer_observer": { + "action": "(relay_south observer observer)", + "duration": 2.0 + }, + "relay-south-dirty_observer_relay": { + "action": "(relay_south observer relay)", + "duration": 2.0 + }, + "relay-south-dirty_observer_south": { + "action": "(relay_south observer south)", + "duration": 2.0 + }, + "relay-south-dirty_relay_north": { + "action": "(relay_south relay north)", + "duration": 2.0 + }, + "relay-south-dirty_relay_observer": { + "action": "(relay_south relay observer)", + "duration": 2.0 + }, + "relay-south-dirty_relay_relay": { + "action": "(relay_south relay relay)", + "duration": 2.0 + }, + "relay-south-dirty_relay_south": { + "action": "(relay_south relay south)", + "duration": 2.0 + }, + "relay-south-dirty_south_north": { + "action": "(relay_south south north)", + "duration": 2.0 + }, + "relay-south-dirty_south_observer": { + "action": "(relay_south south observer)", + "duration": 2.0 + }, + "relay-south-dirty_south_relay": { + "action": "(relay_south south relay)", + "duration": 2.0 + }, + "relay-south-dirty_south_south": { + "action": "(relay_south south south)", + "duration": 2.0 + }, + "scan-north_north": { + "action": "(scan_north north)", + "duration": 3.0 + }, + "scan-north_observer": { + "action": "(scan_north observer)", + "duration": 3.0 + }, + "scan-north_relay": { + "action": "(scan_north relay)", + "duration": 3.0 + }, + "scan-north_south": { + "action": "(scan_north south)", + "duration": 3.0 + }, + "scan-south_north": { + "action": "(scan_south north)", + "duration": 3.0 + }, + "scan-south_observer": { + "action": "(scan_south observer)", + "duration": 3.0 + }, + "scan-south_relay": { + "action": "(scan_south relay)", + "duration": 3.0 + }, + "scan-south_south": { + "action": "(scan_south south)", + "duration": 3.0 + } +} \ No newline at end of file diff --git a/eplansys_demo/pddl/survey-sites.pddl b/eplansys_demo/pddl/survey-sites.pddl new file mode 100644 index 00000000..50d305ce --- /dev/null +++ b/eplansys_demo/pddl/survey-sites.pddl @@ -0,0 +1,104 @@ +;; The classical half of the two-site survey. +;; +;; As in the one-site mission, what the robots know lives in the epistemic +;; task; this file is what the executor needs to drive the actions that task +;; names, and it is deliberately blind to the difference between the open +;; channel and the team link. +;; +;; The two sites are separate predicates rather than a site parameter, which +;; keeps this file in step with the EPDDL beside it, where a parameter can only +;; be an agent. The durations are what the makespan of a policy is measured +;; from: four seconds to drive, three to scan, two to speak. + +(define (domain survey-sites) +(:requirements :strips :typing :adl :durative-actions) + +(:types + robot +) + +(:predicates + (at_depot ?r - robot) + (on_north ?r - robot) + (on_south ?r - robot) + (scanned_north ?r - robot) + (scanned_south ?r - robot) + (told ?r - robot) +) + +(:durative-action goto_north + :parameters (?r - robot) + :duration (= ?duration 4) + :condition (and + (at start (at_depot ?r))) + :effect (and + (at start (not (at_depot ?r))) + (at end (on_north ?r))) +) + +(:durative-action goto_south + :parameters (?r - robot) + :duration (= ?duration 4) + :condition (and + (at start (at_depot ?r))) + :effect (and + (at start (not (at_depot ?r))) + (at end (on_south ?r))) +) + +(:durative-action scan_north + :parameters (?r - robot) + :duration (= ?duration 3) + :condition (and + (over all (on_north ?r))) + :effect (and + (at end (scanned_north ?r))) +) + +(:durative-action scan_south + :parameters (?r - robot) + :duration (= ?duration 3) + :condition (and + (over all (on_south ?r))) + :effect (and + (at end (scanned_south ?r))) +) + +;; One pair for the open channel and one for the team link, per site. They +;; differ in nothing a classical planner can see. +(:durative-action broadcast_north + :parameters (?r - robot) + :duration (= ?duration 2) + :condition (and + (at start (scanned_north ?r))) + :effect (and + (at end (told ?r))) +) + +(:durative-action broadcast_south + :parameters (?r - robot) + :duration (= ?duration 2) + :condition (and + (at start (scanned_south ?r))) + :effect (and + (at end (told ?r))) +) + +(:durative-action relay_north + :parameters (?from ?to - robot) + :duration (= ?duration 2) + :condition (and + (at start (scanned_north ?from))) + :effect (and + (at end (told ?from))) +) + +(:durative-action relay_south + :parameters (?from ?to - robot) + :duration (= ?duration 2) + :condition (and + (at start (scanned_south ?from))) + :effect (and + (at end (told ?from))) +) +) diff --git a/eplansys_demo/src/survey_sites_actions.cpp b/eplansys_demo/src/survey_sites_actions.cpp new file mode 100644 index 00000000..c835db42 --- /dev/null +++ b/eplansys_demo/src/survey_sites_actions.cpp @@ -0,0 +1,141 @@ +// 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. + +// The performers of the two-site survey. +// +// They stand in for hardware and do nothing but wait, as the one-site +// performers do. What is worth watching here is not any one of them but the +// order they run in: two of these drives are for different robots and +// different sites, and a policy dispatched a node at a time still runs them +// one after the other. The wall clock this process reports is what a parallel +// dispatch would have to beat. +// +// Two of them sense, one per site, and each names its own outcome. + +#include +#include + +#include "plansys2_executor/ActionExecutorClient.hpp" + +#include "lifecycle_msgs/msg/transition.hpp" +#include "rclcpp/rclcpp.hpp" + +namespace eplansys_demo +{ + +using namespace std::chrono_literals; // NOLINT (build/namespaces) + +/// A performer that takes a while and then finishes. +/// +/// `outcome` is empty for an ordinary action, and for a sensing one it is the +/// event the robot saw. Naming it on `finish` is the whole of what a performer +/// owes a branching policy. +class SiteAction : public plansys2::ActionExecutorClient +{ +public: + SiteAction( + const std::string & node_name, const std::string & action, + double duration, const std::string & outcome) + : ActionExecutorClient(node_name), duration_(duration), outcome_(outcome) + { + set_parameter(rclcpp::Parameter("action_name", action)); + set_parameter(rclcpp::Parameter("rate", 4.0)); + } + +private: + void do_work() override + { + if (!started_) { + started_ = true; + begun_ = now(); + + RCLCPP_INFO( + get_logger(), "%s: starting (%.1fs)", get_action_name().c_str(), duration_); + } + + const auto elapsed = (now() - begun_).seconds(); + const auto progress = duration_ > 0.0 ? elapsed / duration_ : 1.0; + + if (progress < 1.0) { + send_feedback(static_cast(progress), "working"); + return; + } + + if (outcome_.empty()) { + RCLCPP_INFO(get_logger(), "%s: done", get_action_name().c_str()); + } else { + RCLCPP_INFO( + get_logger(), "%s: observed %s", get_action_name().c_str(), outcome_.c_str()); + } + + finish(true, 1.0, "done", outcome_); + started_ = false; + } + + bool started_{false}; + rclcpp::Time begun_; + double duration_{3.0}; + std::string outcome_; +}; + +} // namespace eplansys_demo + +int main(int argc, char ** argv) +{ + // What each site turns out to hold. Taken from the command line rather than + // from parameters, because the performers share one process and a parameter + // set from a launch file reaches the node it names and not its neighbours. + std::string north = "e-scan-north-dirty"; + std::string south = "e-scan-south-clean"; + for (int i = 1; i + 1 < argc; ++i) { + if (std::string(argv[i]) == "--north") { + north = argv[i + 1]; + } else if (std::string(argv[i]) == "--south") { + south = argv[i + 1]; + } + } + + rclcpp::init(argc, argv); + + auto goto_north = std::make_shared( + "goto_north_node", "goto_north", 4.0, ""); + auto goto_south = std::make_shared( + "goto_south_node", "goto_south", 4.0, ""); + // The two that observe anything, one site each. + auto scan_north = std::make_shared( + "scan_north_node", "scan_north", 3.0, north); + auto scan_south = std::make_shared( + "scan_south_node", "scan_south", 3.0, south); + // Two audiences, per site, indistinguishable from here. + auto broadcast_north = std::make_shared( + "broadcast_north_node", "broadcast_north", 2.0, ""); + auto broadcast_south = std::make_shared( + "broadcast_south_node", "broadcast_south", 2.0, ""); + auto relay_north = std::make_shared( + "relay_north_node", "relay_north", 2.0, ""); + auto relay_south = std::make_shared( + "relay_south_node", "relay_south", 2.0, ""); + + rclcpp::executors::MultiThreadedExecutor executor; + for (const auto & action : {goto_north, goto_south, scan_north, scan_south, + broadcast_north, broadcast_south, relay_north, relay_south}) + { + action->trigger_transition(lifecycle_msgs::msg::Transition::TRANSITION_CONFIGURE); + executor.add_node(action->get_node_base_interface()); + } + + executor.spin(); + rclcpp::shutdown(); + return 0; +} diff --git a/eplansys_demo/src/survey_sites_mission.cpp b/eplansys_demo/src/survey_sites_mission.cpp new file mode 100644 index 00000000..171c5a75 --- /dev/null +++ b/eplansys_demo/src/survey_sites_mission.cpp @@ -0,0 +1,117 @@ +// 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. + +// Starts the two-site survey, and times it. +// +// The mission is the one-site survey twice over, in different hands: each +// scout carries the instrument its own site needs, so the policy has two +// halves that share no agent, no atom and no precondition. The elapsed time +// printed at the end is the number that a parallel dispatch is measured +// against; a serial one pays for both halves. + +#include +#include +#include + +#include "plansys2_msgs/action/execute_plan.hpp" +#include "plansys2_msgs/msg/plan.hpp" +#include "plansys2_executor/ExecutorClient.hpp" +#include "plansys2_planner/PlannerClient.hpp" +#include "plansys2_problem_expert/ProblemExpertClient.hpp" +#include "plansys2_domain_expert/DomainExpertClient.hpp" + +#include "rclcpp/rclcpp.hpp" + +using namespace std::chrono_literals; // NOLINT (build/namespaces) + +int main(int argc, char ** argv) +{ + rclcpp::init(argc, argv); + auto node = rclcpp::Node::make_shared("survey_sites_mission"); + + auto problem = std::make_shared(); + auto planner = std::make_shared(); + auto domain = std::make_shared(); + auto executor = std::make_shared(); + + RCLCPP_INFO(node->get_logger(), "waiting for the planning system"); + for (int i = 0; i < 120 && rclcpp::ok(); ++i) { + if (!domain->getDomain().empty()) { + break; + } + rclcpp::spin_some(node); + std::this_thread::sleep_for(500ms); + } + + // Four robots at the depot. Which of them carries which instrument is not a + // fact this half holds: it decides who can scan what, and so it belongs to + // the epistemic problem, where it is common knowledge. + for (const auto & robot : {"north", "south", "relay", "observer"}) { + problem->addInstance(plansys2::Instance{robot, "robot"}); + problem->addPredicate(plansys2::Predicate("(at_depot " + std::string(robot) + ")")); + } + + // What is wanted, classically: both scouts have reported. Which channel each + // used, and who was allowed to hear it, travels with the EPDDL. + problem->setGoal(plansys2::Goal("(and(told north)(told south))")); + + RCLCPP_INFO(node->get_logger(), "planning"); + const auto plan = planner->getPlan(domain->getDomain(), problem->getProblem()); + if (!plan.has_value()) { + RCLCPP_ERROR(node->get_logger(), "no plan; is the epistemic solver configured?"); + rclcpp::shutdown(); + return 1; + } + + bool branches = false; + for (const auto & item : plan->items) { + branches = branches || item.children.size() > 1; + } + RCLCPP_INFO( + node->get_logger(), "policy with %zu nodes, %s", + plan->items.size(), branches ? "branching" : "linear"); + + if (!executor->start_plan_execution(plan.value())) { + RCLCPP_ERROR(node->get_logger(), "the executor refused the policy"); + rclcpp::shutdown(); + return 1; + } + + RCLCPP_INFO(node->get_logger(), "executing"); + const auto started = node->now(); + + rclcpp::Rate rate(4); + while (rclcpp::ok() && executor->execute_and_check_plan()) { + rclcpp::spin_some(node); + rate.sleep(); + } + + const auto elapsed = (node->now() - started).seconds(); + + const auto result = executor->getResult(); + const bool succeeded = + result && result->result == plansys2_msgs::action::ExecutePlan::Result::SUCCESS; + + if (succeeded) { + // The makespan, which is the point of this mission: the two halves sum to + // eighteen seconds of declared duration run one after another, and to nine + // run at the same time. + RCLCPP_INFO(node->get_logger(), "mission complete in %.1fs", elapsed); + } else { + RCLCPP_ERROR(node->get_logger(), "mission failed after %.1fs", elapsed); + } + + rclcpp::shutdown(); + return succeeded ? 0 : 1; +} From 54499a2af0dcf82200b4c082a156466e263d3776 Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 12:19:10 -0600 Subject: [PATCH 2/7] Dispatch the runs of a policy that were never ordered A policy fixes an order over its actions, and where two consecutive nodes had no reason to be ordered the mission waits for nothing. parallel_groups finds those runs: maximal chains of single continuations whose actions are independent classically, which the builder answers from the domain, and epistemically, which is answered by requiring that the agents the two actions name be disjoint and that neither node's knowledge requirements mention the other's agents. Each run renders as one Parallel, with what followed its last member following the group. Members keep their own guards and their own updates, so a run that was not as independent as it was taken to be fails a guard rather than running past it, and the executor replans. Off unless a deployment asks for it, with epistemic_bt_builder's parallel_dispatch. With it off the tree is the one this builder rendered before the pass existed, which a test pins. --- plansys2_epistemic_bt_builder/CMakeLists.txt | 4 + .../epistemic_bt_builder.hpp | 27 +++ plansys2_epistemic_bt_builder/package.xml | 2 + .../src/epistemic_bt_builder.cpp | 137 +++++++++++- plansys2_epistemic_executor/CMakeLists.txt | 1 + .../plansys2_epistemic_executor/policy_bt.hpp | 23 ++ .../policy_parallel.hpp | 85 ++++++++ plansys2_epistemic_executor/src/policy_bt.cpp | 167 ++++++++++----- .../src/policy_parallel.cpp | 184 ++++++++++++++++ .../test/CMakeLists.txt | 3 + .../test/policy_parallel_test.cpp | 202 ++++++++++++++++++ 11 files changed, 783 insertions(+), 52 deletions(-) create mode 100644 plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp create mode 100644 plansys2_epistemic_executor/src/policy_parallel.cpp create mode 100644 plansys2_epistemic_executor/test/policy_parallel_test.cpp diff --git a/plansys2_epistemic_bt_builder/CMakeLists.txt b/plansys2_epistemic_bt_builder/CMakeLists.txt index 7bd88339..39b70b71 100644 --- a/plansys2_epistemic_bt_builder/CMakeLists.txt +++ b/plansys2_epistemic_bt_builder/CMakeLists.txt @@ -12,6 +12,8 @@ find_package(plansys2_msgs REQUIRED) find_package(plansys2_executor REQUIRED) find_package(plansys2_epistemic_executor REQUIRED) find_package(plansys2_pddl_parser REQUIRED) +find_package(plansys2_domain_expert REQUIRED) +find_package(plansys2_problem_expert REQUIRED) add_library(${PROJECT_NAME} SHARED src/epistemic_bt_builder.cpp) target_include_directories(${PROJECT_NAME} PUBLIC @@ -22,6 +24,8 @@ target_link_libraries(${PROJECT_NAME} plansys2_executor::plansys2_executor plansys2_epistemic_executor::plansys2_epistemic_executor_policy plansys2_pddl_parser::plansys2_pddl_parser + plansys2_domain_expert::plansys2_domain_expert + plansys2_problem_expert::plansys2_problem_expert ${plansys2_msgs_TARGETS} pluginlib::pluginlib rclcpp::rclcpp 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 cf8fca56..b9920597 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 @@ -19,7 +19,9 @@ #include #include +#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_epistemic_executor/policy_bt.hpp" +#include "plansys2_epistemic_executor/policy_parallel.hpp" #include "plansys2_executor/BTBuilder.hpp" namespace plansys2 @@ -43,6 +45,15 @@ namespace plansys2 * of them establish each other's preconditions; for a policy the ordering is * already fixed by the tree, and there is nothing left for a graph to decide. * get_graph() therefore returns nothing, as SequentialBTBuilder's does. + * + * 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. */ class EpistemicBTBuilder : public BTBuilder { @@ -78,8 +89,24 @@ class EpistemicBTBuilder : public BTBuilder bool enable_print_graph = false) override; private: + /// The classical half of the independence test, from the domain: whether + /// either action's effects touch anything the other requires or changes. + /// False whenever the domain cannot answer, since an unknown action is not + /// an independent one. + bool classically_independent( + const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b) const; + std::string bt_action_; int precision_{3}; + + /// Off unless the deployment asks for it. A policy dispatched a group at a + /// time is a change in what runs when, and a system that was tuned around + /// the serial dispatch should not have it changed underneath it by an + /// upgrade. + bool parallel_dispatch_{false}; + + /// Consulted only by the independence test above. + std::shared_ptr domain_client_; plansys2_msgs::msg::Plan plan_; /// Built by get_tree and handed out by get_graph, so that the two describe diff --git a/plansys2_epistemic_bt_builder/package.xml b/plansys2_epistemic_bt_builder/package.xml index b61d58f4..1590a8e9 100644 --- a/plansys2_epistemic_bt_builder/package.xml +++ b/plansys2_epistemic_bt_builder/package.xml @@ -20,6 +20,8 @@ plansys2_executor plansys2_epistemic_executor plansys2_pddl_parser + plansys2_domain_expert + plansys2_problem_expert ament_lint_auto ament_lint_common diff --git a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp index c51f8161..94035c3e 100644 --- a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp +++ b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp @@ -26,7 +26,9 @@ #include #include "plansys2_epistemic_executor/policy.hpp" +#include "plansys2_epistemic_executor/policy_parallel.hpp" #include "plansys2_pddl_parser/AmentIndexCompat.hpp" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 @@ -73,6 +75,43 @@ bool is_a_packaged_plansys2_template(const std::string & content) return false; } +/// Every predicate a requirement or effect tree mentions, grounded, as the +/// name and its arguments. Comparing these is what "touches the same fact" +/// means: two actions that never name a common predicate cannot disturb each +/// other whatever order they run in. +std::set predicates_of(const plansys2_msgs::msg::Tree & tree) +{ + std::set named; + for (const auto & node : tree.nodes) { + if (node.node_type != plansys2_msgs::msg::Node::PREDICATE && + node.node_type != plansys2_msgs::msg::Node::FUNCTION) + { + continue; + } + std::string key = node.name; + for (const auto & param : node.parameters) { + key += " " + param.name; + } + named.insert(key); + } + return named; +} + +void merge(std::set & into, const std::set & from) +{ + into.insert(from.begin(), from.end()); +} + +bool disjoint(const std::set & left, const std::set & right) +{ + for (const auto & one : left) { + if (right.count(one)) { + return false; + } + } + return true; +} + std::string dot_escape(const std::string & text) { std::string out; @@ -93,6 +132,39 @@ void EpistemicBTBuilder::initialize( (void)bt_action_2; // there is one template here: a policy node is a policy node precision_ = precision; + // The parameter is read from a node of this builder's own name, which is how + // a parameters file reaches a plugin that the executor loads without passing + // it anything. Absent the file, and absent the parameter in it, the answer + // is false and the dispatch is the serial one. + // + // Absent a ROS context altogether there is no parameter to read and no + // domain to ask, which is the same answer. Rendering a tree is a pure + // transformation and is tested as one, so a builder that insisted on a + // context would be a builder that could not be tested. + if (rclcpp::ok()) { + try { + auto node = rclcpp::Node::make_shared("epistemic_bt_builder"); + parallel_dispatch_ = node->declare_parameter("parallel_dispatch", false); + if (parallel_dispatch_) { + domain_client_ = std::make_shared(); + } + } catch (const std::exception & error) { + RCLCPP_WARN( + logger(), "could not read parallel_dispatch, dispatching one node at a time: %s", + error.what()); + parallel_dispatch_ = false; + domain_client_.reset(); + } + } else { + parallel_dispatch_ = false; + } + + if (parallel_dispatch_) { + RCLCPP_INFO( + logger(), + "parallel_dispatch is on: independent runs of the policy are dispatched together"); + } + // The executor hands every builder the action template it is configured // with, and with the parameter unset that is PlanSys2's classical default — // which has no CONTINUATIONS placeholder and so no place for the rest of the @@ -142,7 +214,70 @@ std::string EpistemicBTBuilder::get_tree(const plansys2_msgs::msg::Plan & curren logger(), "building a tree for %zu policy nodes%s", policy.size(), policy.branches() ? ", branching" : ""); - return policy_to_bt(policy, bt_action_, precision_); + if (!parallel_dispatch_) { + return policy_to_bt(policy, bt_action_, precision_); + } + + const auto groups = parallel_groups( + policy, + [this](const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b) { + return classically_independent(a, b); + }); + + for (const auto & group : groups) { + std::string named; + for (const auto member : group) { + named += (named.empty() ? "" : ", ") + policy.item(member).action; + } + RCLCPP_INFO(logger(), "dispatching together: %s", named.c_str()); + } + + return policy_to_bt(policy, bt_action_, precision_, groups); +} + +bool EpistemicBTBuilder::classically_independent( + const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b) const +{ + if (!domain_client_) { + return false; + } + + const auto describe = [this](const std::string & expression) { + return domain_client_->getDurativeAction( + plansys2::get_action_name(expression), plansys2::get_action_params(expression)); + }; + + const auto first = describe(a.action); + const auto second = describe(b.action); + if (!first || !second) { + // An action the domain does not describe is one nothing can be concluded + // about, and the conclusion that costs nothing is that it is not + // independent. + return false; + } + + const auto effects = [](const plansys2_msgs::msg::DurativeAction & action) { + auto named = predicates_of(action.at_start_effects); + merge(named, predicates_of(action.at_end_effects)); + return named; + }; + + const auto requirements = [](const plansys2_msgs::msg::DurativeAction & action) { + auto named = predicates_of(action.at_start_requirements); + merge(named, predicates_of(action.over_all_requirements)); + merge(named, predicates_of(action.at_end_requirements)); + return named; + }; + + const auto first_effects = effects(*first); + const auto second_effects = effects(*second); + + // Three ways two actions can be ordered for a reason: one establishes or + // destroys what the other requires, either way round, or both write the same + // fact. None of the three, and the order between them was arbitrary. + return disjoint(first_effects, requirements(*second)) && + disjoint(second_effects, requirements(*first)) && + disjoint(first_effects, second_effects); } namespace diff --git a/plansys2_epistemic_executor/CMakeLists.txt b/plansys2_epistemic_executor/CMakeLists.txt index 8e32d90c..14512e6c 100644 --- a/plansys2_epistemic_executor/CMakeLists.txt +++ b/plansys2_epistemic_executor/CMakeLists.txt @@ -23,6 +23,7 @@ find_package(nlohmann_json 3.10 REQUIRED) add_library(${PROJECT_NAME}_policy SHARED src/policy.cpp src/policy_bt.cpp + src/policy_parallel.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 39fae0bb..26b84488 100644 --- a/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp +++ b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_bt.hpp @@ -18,6 +18,7 @@ #include #include "plansys2_epistemic_executor/policy.hpp" +#include "plansys2_epistemic_executor/policy_parallel.hpp" namespace plansys2 { @@ -99,6 +100,28 @@ std::string policy_to_bt( const std::string & action_bt = "", int precision = 3); +/** + * @brief Render a policy whose independent runs are dispatched together. + * + * The same rendering, except that every group named here becomes one + * `Parallel` holding the members' blocks, and what followed the last member + * follows the group. A group of one, or none at all, renders exactly as the + * overload above: this is a strict extension, and the tree for a policy with + * no groups is byte for byte the tree that was rendered before there were any. + * + * The members keep their own guards and their own updates. A group whose + * members were not as independent as they were taken to be therefore fails a + * guard rather than running past it, which the executor answers by replanning. + * + * @param[in] groups Runs of policy nodes to dispatch together, as + * `parallel_groups` computes them. + */ +std::string policy_to_bt( + const Policy & policy, + const std::string & action_bt, + int precision, + const ParallelGroups & groups); + /// 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 new file mode 100644 index 00000000..49658e77 --- /dev/null +++ b/plansys2_epistemic_executor/include/plansys2_epistemic_executor/policy_parallel.hpp @@ -0,0 +1,85 @@ +// 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_PARALLEL_HPP_ +#define PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_PARALLEL_HPP_ + +#include +#include +#include +#include + +#include "plansys2_epistemic_executor/policy.hpp" + +namespace plansys2 +{ + +/// Policy nodes that may be dispatched at the same time, in policy order. +using ParallelGroup = std::vector; + +/// Every group of more than one node. Nodes not named here are dispatched one +/// at a time, as they always were. +using ParallelGroups = std::vector; + +/// Whether two policy nodes' classical actions can overlap: whether neither +/// one's effects disturb what the other requires. Supplied by the caller, +/// because answering it means reading the PDDL domain and this library holds +/// no domain. +using Independence = + std::function; + +/** + * @brief The runs of a policy that could be dispatched together. + * + * A policy is dispatched a node at a time: the tree the executor runs is a + * chain of sequences, and the second action of a mission starts when the first + * has finished whether or not it had any reason to wait. Where the two are + * independent the mission pays for that wait and buys nothing. + * + * This finds the runs where it buys nothing. A run is a maximal chain of + * policy nodes, each the only continuation of the one before it, whose actions + * are pairwise independent in both of these senses: + * + * - classically, which `independent` answers from the domain, and + * + * - epistemically, which is answered here and conservatively: the agents the + * two actions name must be disjoint, and neither node's knowledge + * requirements may mention an agent the other names. An action about one + * set of agents cannot establish or destroy a knowledge requirement about + * another set, so the guards of the members hold at the moment the group + * starts exactly as they held one after another. + * + * The second test is exact for ontic actions and for private and semi-private + * ones, whose event models reach the agents they name. It is not exact for a + * public announcement, which reaches every agent whether or not the action + * names it, and whose update can therefore bear on a requirement this test + * calls independent. That case is left to fail loudly rather than silently: + * every member keeps its own `CheckKnowledge`, so a guard that should have + * been established by another member fails the group, which fails the tree and + * sends the executor to replan from the state the mission reached. + * + * Only the last node of a run may branch. A node with several continuations + * ends its run, because the continuation is chosen by what the node observed + * and cannot be started before it is known. + */ +ParallelGroups parallel_groups(const Policy & policy, 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. +std::vector item_agents(const plansys2_msgs::msg::PlanItem & item); + +} // namespace plansys2 + +#endif // PLANSYS2_EPISTEMIC_EXECUTOR__POLICY_PARALLEL_HPP_ diff --git a/plansys2_epistemic_executor/src/policy_bt.cpp b/plansys2_epistemic_executor/src/policy_bt.cpp index a6d3cc6e..505384f4 100644 --- a/plansys2_epistemic_executor/src/policy_bt.cpp +++ b/plansys2_epistemic_executor/src/policy_bt.cpp @@ -14,6 +14,9 @@ #include "plansys2_epistemic_executor/policy_bt.hpp" +#include +#include + #include #include #include @@ -103,68 +106,130 @@ std::string policy_action_id(const plansys2_msgs::msg::PlanItem & item, int prec } std::string policy_to_bt(const Policy & policy, const std::string & action_bt, int precision) +{ + return policy_to_bt(policy, action_bt, precision, ParallelGroups{}); +} + +std::string policy_to_bt( + const Policy & policy, const std::string & action_bt, int precision, + const ParallelGroups & groups) { const std::string tmpl = action_bt.empty() ? kDefaultEpistemicActionBT : action_bt; + // Which group a node heads, and which nodes are inside one. A node inside a + // group other than its head is never rendered on its own: it is reached by + // the group it belongs to. + std::map heads; + std::set inside; + for (const auto & group : groups) { + if (group.size() < 2) { + continue; + } + heads[group.front()] = &group; + for (const auto member : group) { + inside.insert(member); + } + } + // 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 { - const auto & item = policy.item(index); - const std::string action_id = policy_action_id(item, precision); - const std::string node_id = std::to_string(index); - // One blackboard entry per node: two sensing actions in flight on - // different branches never share a key, and the name says which node - // wrote it when reading a Groot trace. - const std::string outcome_key = "epistemic_outcome_" + node_id; - // Where the performer's report lands on its way from ExecuteAction to - // the update. Distinct from outcome_key: this is what the robot says it - // saw, and that one 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 std::string observed_key = "epistemic_observed_" + node_id; - - std::string continuations; - - const auto only = policy.only_successor(index); - if (only) { - // Nothing to choose: the continuation simply follows. This is what - // makes a classical plan render as the flat sequence PlanSys2 builds. - continuations = self(*only, self); - } else if (item.children.size() > 1) { - std::string branches; - for (std::size_t i = 0; i < item.children.size(); ++i) { - branches += indent(1) + "\n"; - if (item.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(item.children[i], self), 1); + // 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 + // 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) { + // 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); } - } - std::string outcome_list; - for (std::size_t i = 0; i < item.outcomes.size(); ++i) { - outcome_list += (i ? ";" : "") + item.outcomes[i]; + if (node.children.size() <= 1) { + 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"; + }; + + // 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"; + }; + + 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); } - continuations = - "\n" + - branches + - "\n"; + // 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"; } - // Otherwise this node ends the policy and nothing follows it. - - std::string block = tmpl; - replace_all(block, "NODE_NAME", escape("node_" + node_id + " " + item.action)); - replace_all(block, "ACTION_ID", escape(action_id)); - replace_all(block, "NODE_ID", node_id); - replace_all(block, "OUTCOME_KEY", outcome_key); - replace_all(block, "OBSERVED_KEY", observed_key); - replace_all(block, "CONTINUATIONS", shift(continuations, 1)); - return block + "\n"; + + return block_of(index, continuations_of(index)); }; std::string body; diff --git a/plansys2_epistemic_executor/src/policy_parallel.cpp b/plansys2_epistemic_executor/src/policy_parallel.cpp new file mode 100644 index 00000000..3f823aad --- /dev/null +++ b/plansys2_epistemic_executor/src/policy_parallel.cpp @@ -0,0 +1,184 @@ +// 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_parallel.hpp" + +#include +#include +#include +#include +#include + +namespace plansys2 +{ + +namespace +{ + +/// The parts of a grounded epistemic name after the schema, which are its +/// arguments and, in EPDDL, are agents: `relay-north-dirty_north_relay` names +/// `north` and `relay`. +void grounded_arguments(const std::string & name, std::vector & out) +{ + std::size_t at = name.find('_'); + while (at != std::string::npos) { + const auto start = at + 1; + at = name.find('_', start); + const auto part = name.substr(start, at == std::string::npos ? at : at - start); + if (!part.empty()) { + out.push_back(part); + } + } +} + +/// The objects of a PDDL action expression: `(relay_north north relay)`. +void expression_arguments(const std::string & expression, std::vector & out) +{ + std::string token; + bool first = true; + for (const char c : expression) { + if (c == '(' || c == ')' || std::isspace(static_cast(c))) { + if (!token.empty()) { + if (!first) { // the first token is the action's name + out.push_back(token); + } + first = false; + token.clear(); + } + continue; + } + token += c; + } + if (!token.empty() && !first) { + out.push_back(token); + } +} + +/// Whether `text` contains `name` as a word rather than inside a longer one, +/// so that an agent called `north` is not found in `north-west`. +bool mentions(const std::string & text, const std::string & name) +{ + const auto word = [](char c) { + return std::isalnum(static_cast(c)) || c == '_' || c == '-'; + }; + + std::size_t at = text.find(name); + while (at != std::string::npos) { + const bool before = at == 0 || !word(text[at - 1]); + const auto end = at + name.size(); + const bool after = end >= text.size() || !word(text[end]); + if (before && after) { + return true; + } + at = text.find(name, at + 1); + } + return false; +} + +/// The epistemic half of the test. See the header for what it does and does +/// not establish. +bool epistemically_independent( + const plansys2_msgs::msg::PlanItem & a, const plansys2_msgs::msg::PlanItem & b) +{ + const auto agents_a = item_agents(a); + const auto agents_b = item_agents(b); + + for (const auto & one : agents_a) { + if (std::find(agents_b.begin(), agents_b.end(), one) != agents_b.end()) { + return false; + } + for (const auto & requirement : b.knowledge_requirements) { + if (mentions(requirement, one)) { + return false; + } + } + } + + for (const auto & other : agents_b) { + for (const auto & requirement : a.knowledge_requirements) { + if (mentions(requirement, other)) { + return false; + } + } + } + + return true; +} + +} // namespace + +std::vector item_agents(const plansys2_msgs::msg::PlanItem & item) +{ + std::vector names; + grounded_arguments(item.epistemic_action, names); + expression_arguments(item.action, names); + + std::sort(names.begin(), names.end()); + names.erase(std::unique(names.begin(), names.end()), names.end()); + return names; +} + +ParallelGroups parallel_groups(const Policy & policy, const Independence & independent) +{ + ParallelGroups groups; + if (policy.empty() || policy.sequential()) { + // A plan that names no continuations is PlanSys2's own sequence, and + // PlanSys2 has a builder that parallelises those already. + return groups; + } + + std::set taken; + + for (const auto index : policy.preorder()) { + if (taken.count(index)) { + continue; + } + + ParallelGroup group{index}; + auto current = index; + + while (const auto next = policy.only_successor(current)) { + if (*next == plansys2_msgs::msg::PlanItem::POLICY_DONE || taken.count(*next)) { + break; + } + + const auto & candidate = policy.item(*next); + const bool joins = std::all_of( + group.begin(), group.end(), + [&](std::uint32_t member) { + const auto & held = policy.item(member); + return epistemically_independent(held, candidate) && + (!independent || independent(held, candidate)); + }); + + if (!joins) { + break; + } + + group.push_back(*next); + current = *next; + } + + if (group.size() > 1) { + for (const auto member : group) { + taken.insert(member); + } + groups.push_back(group); + } + } + + return groups; +} + +} // namespace plansys2 diff --git a/plansys2_epistemic_executor/test/CMakeLists.txt b/plansys2_epistemic_executor/test/CMakeLists.txt index 5cb2dffc..b58d3eee 100644 --- a/plansys2_epistemic_executor/test/CMakeLists.txt +++ b/plansys2_epistemic_executor/test/CMakeLists.txt @@ -4,6 +4,9 @@ target_link_libraries(policy_test ${PROJECT_NAME}_policy) ament_add_gtest(policy_bt_test policy_bt_test.cpp TIMEOUT 120) target_link_libraries(policy_bt_test ${PROJECT_NAME}_policy behaviortree_cpp::behaviortree_cpp) +ament_add_gtest(policy_parallel_test policy_parallel_test.cpp TIMEOUT 120) +target_link_libraries(policy_parallel_test ${PROJECT_NAME}_policy) + # 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_parallel_test.cpp b/plansys2_epistemic_executor/test/policy_parallel_test.cpp new file mode 100644 index 00000000..0b3eb6cb --- /dev/null +++ b/plansys2_epistemic_executor/test/policy_parallel_test.cpp @@ -0,0 +1,202 @@ +// 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 "plansys2_epistemic_executor/policy_bt.hpp" +#include "plansys2_epistemic_executor/policy_parallel.hpp" + +using plansys2::ParallelGroups; +using plansys2::Policy; +using plansys2::item_agents; +using plansys2::parallel_groups; +using plansys2::policy_to_bt; +using PlanItem = plansys2_msgs::msg::PlanItem; + +namespace +{ + +PlanItem action( + const std::string & expression, const std::string & grounded, float time, + const std::vector & children = {}, + const std::vector & outcomes = {}) +{ + PlanItem item; + item.action = expression; + item.epistemic_action = grounded; + item.time = time; + item.duration = 1.0f; + item.children = children; + item.outcomes = outcomes; + item.sensing = children.size() > 1; + return item; +} + +/// The two halves of the two-site survey, in the order the planner returns +/// them: a drive and a scan at one site, then a report, then the same at the +/// other. The report and the second drive are the pair with no reason to be +/// ordered. +plansys2_msgs::msg::Plan two_site_policy() +{ + plansys2_msgs::msg::Plan plan; + plan.epistemic_goal = "(and (Kw north dirty-north) (Kw south dirty-south))"; + plan.items = { + action("(goto_north north)", "goto-north_north", 0.0f, {1}), + action( + "(scan_north north)", "scan-north_north", 4.0f, {2, 2}, + {"e-scan-north-dirty", "e-scan-north-clean"}), + action("(relay_north north relay)", "relay-north-dirty_north_relay", 7.0f, {3}), + action("(goto_south south)", "goto-south_south", 9.0f, {4}), + action("(scan_south south)", "scan-south_south", 13.0f, {}), + }; + return plan; +} + +/// Everything is independent. Isolating the epistemic half of the test. +const plansys2::Independence kAlways = + [](const PlanItem &, const PlanItem &) {return true;}; + +const plansys2::Independence kNever = + [](const PlanItem &, const PlanItem &) {return false;}; + +} // namespace + +TEST(PolicyParallel, agents_come_from_both_vocabularies) +{ + const auto named = item_agents( + action("(relay_north north relay)", "relay-north-dirty_north_relay", 0.0f)); + + EXPECT_EQ(named.size(), 2u); + EXPECT_NE(std::find(named.begin(), named.end(), "north"), named.end()); + EXPECT_NE(std::find(named.begin(), named.end(), "relay"), named.end()); +} + +TEST(PolicyParallel, a_run_of_independent_nodes_is_one_group) +{ + const Policy policy(two_site_policy()); + const auto groups = parallel_groups(policy, kAlways); + + ASSERT_EQ(groups.size(), 1u); + EXPECT_EQ(groups.front(), (plansys2::ParallelGroup{2, 3})); +} + +TEST(PolicyParallel, a_shared_agent_ends_a_group) +{ + // The drive and the scan at the south site are the same robot's, so they + // stay ordered however independent their classical halves look. + const Policy policy(two_site_policy()); + const auto groups = parallel_groups(policy, kAlways); + + ASSERT_EQ(groups.size(), 1u); + EXPECT_EQ(groups.front().back(), 3u); +} + +TEST(PolicyParallel, a_branch_ends_a_group) +{ + // Node 1 branches, so nothing after it joins node 0: which continuation runs + // is not known until it has been observed. + const Policy policy(two_site_policy()); + const auto groups = parallel_groups(policy, kAlways); + + for (const auto & group : groups) { + EXPECT_EQ(std::find(group.begin(), group.end(), 0u), group.end()); + } +} + +TEST(PolicyParallel, the_classical_test_can_refuse_every_group) +{ + const Policy policy(two_site_policy()); + EXPECT_TRUE(parallel_groups(policy, kNever).empty()); +} + +TEST(PolicyParallel, a_knowledge_requirement_naming_the_other_agent_refuses) +{ + auto plan = two_site_policy(); + plan.items[3].knowledge_requirements = {"(K south (told north))"}; + + const Policy policy(plan); + EXPECT_TRUE(parallel_groups(policy, kAlways).empty()); +} + +TEST(PolicyParallel, a_sequential_plan_has_no_groups) +{ + // A plan that names no continuations is PlanSys2's own sequence, which has a + // builder of its own for this. + plansys2_msgs::msg::Plan plan; + plan.items = { + action("(goto_north north)", "goto-north_north", 0.0f), + action("(goto_south south)", "goto-south_south", 4.0f), + }; + + const Policy policy(plan); + EXPECT_TRUE(parallel_groups(policy, kAlways).empty()); +} + +TEST(PolicyParallel, the_group_is_rendered_as_one_parallel) +{ + const Policy policy(two_site_policy()); + const auto tree = policy_to_bt(policy, "", 3, parallel_groups(policy, kAlways)); + + EXPECT_NE(tree.find(""), std::string::npos); + // Both members are inside it, and what followed the last of them follows the + // group rather than either member. + const auto parallel_at = tree.find(""); + ASSERT_NE(closed_at, std::string::npos); + EXPECT_LT(tree.find("(relay_north north relay)"), closed_at); + EXPECT_LT(tree.find("(goto_south south)"), closed_at); + EXPECT_GT(tree.find("(scan_south south)"), parallel_at); +} + +TEST(PolicyParallel, a_group_inside_a_branch_is_one_child_of_the_switch) +{ + // The switch indexes its branches by its outcomes, so a branch that rendered + // as two elements would leave the tree claiming more branches than the + // policy has outcomes. The group and its continuation are one element. + const Policy policy(two_site_policy()); + const auto tree = policy_to_bt(policy, "", 3, parallel_groups(policy, kAlways)); + + const auto switch_at = tree.find("", switch_at); + ASSERT_NE(switch_end, std::string::npos); + + const auto body = tree.substr(switch_at, switch_end - switch_at); + // One sequence per outcome, and the two outcomes of this policy lead to the + // same node, so the rendering repeats it: two group sequences, no more. + std::size_t groups_rendered = 0; + for (std::size_t at = body.find(" Date: Tue, 22 Sep 2026 12:19:36 -0600 Subject: [PATCH 3/7] Document the two-site survey and what dispatching it together saves --- eplansys_docs/demos/index.rst | 1 + eplansys_docs/demos/two_sites.rst | 88 +++++++++++++++++++++++++++++++ 2 files changed, 89 insertions(+) create mode 100644 eplansys_docs/demos/two_sites.rst diff --git a/eplansys_docs/demos/index.rst b/eplansys_docs/demos/index.rst index e0e9db8a..0da21fd8 100644 --- a/eplansys_docs/demos/index.rst +++ b/eplansys_docs/demos/index.rst @@ -10,3 +10,4 @@ cannot express. corridor survey + two_sites diff --git a/eplansys_docs/demos/two_sites.rst b/eplansys_docs/demos/two_sites.rst new file mode 100644 index 00000000..423e80de --- /dev/null +++ b/eplansys_docs/demos/two_sites.rst @@ -0,0 +1,88 @@ +The two-site survey +=================== + +Two sites, two scouts, and a policy whose halves were never ordered. + +This is :doc:`survey` run twice, in different hands. Two sites may be +contaminated, independently; ``north`` and ``south`` carry the instruments their +own sites read, ``relay`` is the team member both findings have to reach, and +``observer`` is the machine the mission still excludes. The six goal conjuncts +are the survey's three, once per site. + +Nothing links the two halves. They name different atoms, different robots and +different places, and no action of one appears in a precondition of the other. +What the mission is for is the consequence: a policy dispatched a node at a +time runs eighteen seconds of declared duration end to end, and the halves of +it never overlap although nothing asked them not to. + +Running it +---------- + +.. code-block:: bash + + ros2 launch eplansys_demo survey_sites_launch.py + ros2 launch eplansys_demo survey_sites_launch.py north:=clean south:=dirty + ros2 launch eplansys_demo survey_sites_launch.py parallel:=true + +``north:`` and ``south:`` say what each site turns out to hold, and the policy +takes a different branch for each of the four combinations. ``parallel:=true`` +turns on the dispatch described below. It is the same policy either way. + +Like the survey, the mission is stated in EPDDL and ground by ``plank`` at start +up, so ``plank`` has to be built and on ``PATH``. + +The goal +-------- + +.. code-block:: lisp + + (:goal + (and + ([Kw. north] (contaminated-north)) + ([Kw. relay] (contaminated-north)) + ( (contaminated-north)) + ([Kw. south] (contaminated-south)) + ([Kw. relay] (contaminated-south)) + ( (contaminated-south)))) + +Why the instruments +------------------- + +The instruments are not decoration. Without them the cheapest policy sends one +robot to both sites, which answers the mission correctly and leaves nothing to +overlap: one robot cannot be in two places whatever the dispatch. Declaring +that only ``north`` reads the north site, as common knowledge, is what puts the +two halves in different hands. + +The same goes for what the initial state leaves out. An instrument atom the +problem does not settle is one the model has to be uncertain about, and eight +such atoms designate two hundred and fifty-six worlds where four would do. Every +agent is therefore named for every instrument. + +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. + +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``. In the +policy this mission produces, the report from the north site and the drive to +the south one are such a run, and the mission takes about twenty-one and a half +seconds. + +The saving is the shorter of the two overlapped actions, and it is smaller than +the halves of the mission would allow. A policy orders its nodes, and the two +halves of this one are interleaved rather than adjacent: the drive south sits +below the branch on what the north scan found, so only the pair that happens to +be consecutive can be grouped. Lifting an independent action across a branch +point, so that both halves run from the start, is what the pass does not yet do. + +.. code-block:: text + + dispatching together: (relay_north north relay), (goto_south south) + +is what it says when it finds one. From 7189fc2f2eefe76c6f8458d4c811038df3162779 Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 12:37:42 -0600 Subject: [PATCH 4/7] Drop a set nothing reads --- plansys2_epistemic_executor/src/policy_bt.cpp | 16 +++++----------- 1 file changed, 5 insertions(+), 11 deletions(-) diff --git a/plansys2_epistemic_executor/src/policy_bt.cpp b/plansys2_epistemic_executor/src/policy_bt.cpp index 505384f4..362bf06c 100644 --- a/plansys2_epistemic_executor/src/policy_bt.cpp +++ b/plansys2_epistemic_executor/src/policy_bt.cpp @@ -15,7 +15,6 @@ #include "plansys2_epistemic_executor/policy_bt.hpp" #include -#include #include #include @@ -116,18 +115,13 @@ std::string policy_to_bt( { const std::string tmpl = action_bt.empty() ? kDefaultEpistemicActionBT : action_bt; - // Which group a node heads, and which nodes are inside one. A node inside a - // group other than its head is never rendered on its own: it is reached by - // the group it belongs to. + // 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; - std::set inside; for (const auto & group : groups) { - if (group.size() < 2) { - continue; - } - heads[group.front()] = &group; - for (const auto member : group) { - inside.insert(member); + if (group.size() > 1) { + heads[group.front()] = &group; } } From 19f61b522cc72b96bb8e2eafc4c6775dbf02cc45 Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 12:39:54 -0600 Subject: [PATCH 5/7] Keep the domain client out of the builder's header The header is included by tests that have no domain to talk to. Pulling the client in with it changed what they compile against, and on rolling the test that asserts a graph is not null stopped compiling: gtest picks a printer for the type, and the one it picked there does not exist. Forward declared instead, with the destructor out of line so that a shared_ptr to an incomplete type is legal where the builder is destroyed. --- .../epistemic_bt_builder.hpp | 9 +++++++-- .../src/epistemic_bt_builder.cpp | 3 +++ 2 files changed, 10 insertions(+), 2 deletions(-) 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 b9920597..12492857 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 @@ -19,14 +19,14 @@ #include #include -#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_epistemic_executor/policy_bt.hpp" -#include "plansys2_epistemic_executor/policy_parallel.hpp" #include "plansys2_executor/BTBuilder.hpp" namespace plansys2 { +class DomainExpertClient; + /** * @class plansys2::EpistemicBTBuilder * @brief The BTBuilder that renders a policy rather than a sequence. @@ -60,6 +60,11 @@ class EpistemicBTBuilder : public BTBuilder public: EpistemicBTBuilder() = default; + /// Out of line, because the domain client is only forward declared here: + /// this header is included by tests that have no domain to talk to, and + /// dragging its client in changes what they compile against. + ~EpistemicBTBuilder(); + void initialize( const std::string & bt_action_1 = "", const std::string & bt_action_2 = "", diff --git a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp index 94035c3e..5a461b64 100644 --- a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp +++ b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp @@ -25,6 +25,7 @@ #include #include +#include "plansys2_domain_expert/DomainExpertClient.hpp" #include "plansys2_epistemic_executor/policy.hpp" #include "plansys2_epistemic_executor/policy_parallel.hpp" #include "plansys2_pddl_parser/AmentIndexCompat.hpp" @@ -126,6 +127,8 @@ std::string dot_escape(const std::string & text) } // namespace +EpistemicBTBuilder::~EpistemicBTBuilder() = default; + void EpistemicBTBuilder::initialize( const std::string & bt_action_1, const std::string & bt_action_2, int precision) { From 1a5d5c01a74f5993d3f75c31a9d748cac183810d Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 13:10:01 -0600 Subject: [PATCH 6/7] Say the two halves of the test as two statements Rolling's uncrustify indents the continuation of that return differently from Humble's, and neither of them is worth arguing with. --- plansys2_epistemic_executor/src/policy_parallel.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/plansys2_epistemic_executor/src/policy_parallel.cpp b/plansys2_epistemic_executor/src/policy_parallel.cpp index 3f823aad..06c4a8d4 100644 --- a/plansys2_epistemic_executor/src/policy_parallel.cpp +++ b/plansys2_epistemic_executor/src/policy_parallel.cpp @@ -158,8 +158,10 @@ ParallelGroups parallel_groups(const Policy & policy, const Independence & indep group.begin(), group.end(), [&](std::uint32_t member) { const auto & held = policy.item(member); - return epistemically_independent(held, candidate) && - (!independent || independent(held, candidate)); + if (!epistemically_independent(held, candidate)) { + return false; + } + return !independent || independent(held, candidate); }); if (!joins) { From 54cbe71d5b1831c847fb44020305cb6d24714a68 Mon Sep 17 00:00:00 2001 From: HanielUlises Date: Tue, 22 Sep 2026 13:25:57 -0600 Subject: [PATCH 7/7] Report the three runs the pass finds, and the seventeen seconds Measured against the pinned planner rather than a working copy of it: the policy 0.2.0 returns puts the two drives next to each other, which the earlier figure's policy did not, and the mission drops from twenty- four seconds to seventeen. --- eplansys_docs/demos/two_sites.rst | 25 ++++++++++++------------- 1 file changed, 12 insertions(+), 13 deletions(-) diff --git a/eplansys_docs/demos/two_sites.rst b/eplansys_docs/demos/two_sites.rst index 423e80de..51b88f3f 100644 --- a/eplansys_docs/demos/two_sites.rst +++ b/eplansys_docs/demos/two_sites.rst @@ -69,20 +69,19 @@ 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``. In the -policy this mission produces, the report from the north site and the drive to -the south one are such a run, and the mission takes about twenty-one and a half -seconds. - -The saving is the shorter of the two overlapped actions, and it is smaller than -the halves of the mission would allow. A policy orders its nodes, and the two -halves of this one are interleaved rather than adjacent: the drive south sits -below the branch on what the north scan found, so only the pair that happens to -be consecutive can be grouped. Lifting an independent action across a branch -point, so that both halves run from the start, is what the pass does not yet do. +they name be disjoint --- and renders each run as one ``Parallel``. It says +which runs it found: .. code-block:: text - dispatching together: (relay_north north relay), (goto_south south) + dispatching together: (goto_north north), (goto_south south) + dispatching together: (relay_north north relay), (scan_south south) -is what it says when it finds one. +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.