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; +} 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..51b88f3f --- /dev/null +++ b/eplansys_docs/demos/two_sites.rst @@ -0,0 +1,87 @@ +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``. It says +which runs it found: + +.. code-block:: text + + dispatching together: (goto_north north), (goto_south south) + dispatching together: (relay_north north relay), (scan_south south) + +The two drives leave together, and the report from the north site overlaps the +scan of the south one. The mission takes about seventeen seconds. + +Seventeen is not nine, which is what the two halves would take if each ran from +the start. The scan of the north site is where the policy branches, and nothing +below a branch can begin before the branch is resolved, so the south half waits +for a scan it has no stake in. Lifting an independent action across a branch +point is what the pass does not do. 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..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 @@ -25,6 +25,8 @@ namespace plansys2 { +class DomainExpertClient; + /** * @class plansys2::EpistemicBTBuilder * @brief The BTBuilder that renders a policy rather than a sequence. @@ -43,12 +45,26 @@ 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 { 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 = "", @@ -78,8 +94,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..5a461b64 100644 --- a/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp +++ b/plansys2_epistemic_bt_builder/src/epistemic_bt_builder.cpp @@ -25,8 +25,11 @@ #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" +#include "plansys2_problem_expert/Utils.hpp" #include "rclcpp/rclcpp.hpp" namespace plansys2 @@ -73,6 +76,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; @@ -87,12 +127,47 @@ 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) { (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 +217,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..362bf06c 100644 --- a/plansys2_epistemic_executor/src/policy_bt.cpp +++ b/plansys2_epistemic_executor/src/policy_bt.cpp @@ -14,6 +14,8 @@ #include "plansys2_epistemic_executor/policy_bt.hpp" +#include + #include #include #include @@ -103,68 +105,125 @@ 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. The rest of a group's members are never rendered + // on their own: the chain that led to them is what the group replaces, and + // they are reached by the head. + std::map heads; + for (const auto & group : groups) { + if (group.size() > 1) { + heads[group.front()] = &group; + } + } + // 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..06c4a8d4 --- /dev/null +++ b/plansys2_epistemic_executor/src/policy_parallel.cpp @@ -0,0 +1,186 @@ +// 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); + if (!epistemically_independent(held, candidate)) { + return false; + } + return !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("