Survey planner: respect when problem instance specifies actions were …

…completed (#132)

astrobee/survey_manager/survey_planner/data/test_cases/README.md

@@ -0,0 +1,13 @@
+
+This folder contains informal test cases for `problem_generator.py` and `plan_survey.py`. The
+`*_config.yaml` test cases and the `generate.bash` script are precious source, and all other files
+were auto-generated from the test cases and committed so they could be compared to the output of
+future runs.
+
+To run the informal tests, call `generate.bash` in this folder, which will overwrite the committed
+version of the generated files, then run `git diff` to check for differences in the new outputs
+versus their committed version.
+
+Typically we don't expect any differences, but future code changes could change the expected output
+and that might not be an error. Observed changes require informed manual review of correctness. If
+the new output is correct, it should probably be committed.

astrobee/survey_manager/survey_planner/data/test_cases/completed1_config.yaml

@@ -0,0 +1,38 @@
+# Copyright (c) 2023, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+# platform" software is 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.
+
+# Test case for marking goals completed. For goals with completed: true, we should see completed-x
+# and need-x predicates asserted in the problem_generator.py output, and those goals should not
+# result in any actions in the plan. This is a fairly minimal test case verifying we can mark both
+# panoramas and stereos as completed.
+
+goals:
+
+- {type: panorama, robot: bumble, order: 0, location: jem_bay4, completed: true}
+- {type: panorama, robot: bumble, order: 1, location: jem_bay3}
+- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3}
+
+- {type: panorama, robot: honey, order: 1, location: jem_bay7, completed: true}
+- {type: panorama, robot: honey, order: 2, location: jem_bay6}
+- {type: stereo, robot: honey, order: 4, trajectory: jem_bay7_to_bay4, completed: true}
+
+init:
+  bumble:
+    location: berth1
+  honey:
+    location: berth2

astrobee/survey_manager/survey_planner/data/test_cases/completed1_plan.pddl

@@ -0,0 +1,14 @@
+; Solution Found
+0.000: (undock bumble berth1 jem_bay7 jem_bay6 jem_bay8) [30.000]
+30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000]
+50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000]
+70.003: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000]
+70.004: (undock honey berth2 jem_bay7 jem_bay6 jem_bay8) [30.000]
+90.004: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000]
+100.005: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000]
+110.005: (panorama bumble o1 jem_bay3) [780.000]
+120.006: (panorama honey o2 jem_bay6) [780.000]
+890.006: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000]
+910.007: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000]
+930.008: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) [600.000]
+

astrobee/survey_manager/survey_planner/data/test_cases/completed1_plan.yaml

@@ -0,0 +1,115 @@
+- start_time_seconds: '0.000'
+  action:
+    type: undock
+    robot: bumble
+  duration_seconds: '30.000'
+- start_time_seconds: '30.001'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay7
+    to_name: jem_bay6
+    to_pos:
+    - 11.0
+    - -9.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '50.002'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay6
+    to_name: jem_bay5
+    to_pos:
+    - 11.0
+    - -8.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '70.003'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay5
+    to_name: jem_bay4
+    to_pos:
+    - 11.0
+    - -7.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '70.004'
+  action:
+    type: undock
+    robot: honey
+  duration_seconds: '30.000'
+- start_time_seconds: '90.004'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay4
+    to_name: jem_bay3
+    to_pos:
+    - 11.0
+    - -6.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '100.005'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay7
+    to_name: jem_bay6
+    to_pos:
+    - 11.0
+    - -9.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '110.005'
+  action:
+    type: panorama
+    robot: bumble
+    location_name: jem_bay3
+    location_pos:
+    - 11.0
+    - -6.0
+    - 4.8
+  duration_seconds: '780.000'
+- start_time_seconds: '120.006'
+  action:
+    type: panorama
+    robot: honey
+    location_name: jem_bay6
+    location_pos:
+    - 11.0
+    - -9.0
+    - 4.8
+  duration_seconds: '780.000'
+- start_time_seconds: '890.006'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay3
+    to_name: jem_bay2
+    to_pos:
+    - 11.0
+    - -5.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '910.007'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay2
+    to_name: jem_bay1
+    to_pos:
+    - 11.0
+    - -4.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '930.008'
+  action:
+    type: stereo
+    robot: bumble
+    fplan: ISAAC/jem_stereo_mapping_bay1_to_bay3
+    base_name: jem_bay1
+    bound_name: jem_bay4
+  duration_seconds: '600.000'

astrobee/survey_manager/survey_planner/data/test_cases/completed1_problem.pddl

@@ -0,0 +1,160 @@
+;; Auto-generated by problem_generator.py. Do not edit!
+;; Command was: src/survey_planner/problem_generator.py --config=data/survey_static.yaml,data/test_cases/completed1_config.yaml --output=data/test_cases/completed1_problem.pddl
+;; Working directory was: data/test_cases
+;; Problem template: ../../pddl/jem_survey_template.pddl
+;; Config 1: data/survey_static.yaml
+;; Config 2: data/test_cases/completed1_config.yaml
+
+(define (problem jem-survey)
+    (:domain survey-manager)
+    (:metric minimize (total-time))
+    (:objects
+        jem_bay0 jem_bay1 jem_bay2 jem_bay3 jem_bay4 jem_bay5 jem_bay6 jem_bay7 jem_bay8 berth1 berth2 - location
+        bumble honey - robot
+        o0 o1 o2 o3 o4 - order
+    )
+
+    (:goal
+        (and
+            (completed-panorama bumble o0 jem_bay4)
+            (completed-panorama bumble o1 jem_bay3)
+            (completed-stereo bumble o4 jem_bay1 jem_bay4)
+            (completed-panorama honey o1 jem_bay7)
+            (completed-panorama honey o2 jem_bay6)
+            (completed-stereo honey o4 jem_bay7 jem_bay4)
+        )
+    )
+
+    (:init
+        ;; === Static predicates ===
+        (move-connected jem_bay0 jem_bay1)
+        (move-connected jem_bay1 jem_bay0)
+        (move-connected jem_bay1 jem_bay2)
+        (move-connected jem_bay2 jem_bay1)
+        (move-connected jem_bay2 jem_bay3)
+        (move-connected jem_bay3 jem_bay2)
+        (move-connected jem_bay3 jem_bay4)
+        (move-connected jem_bay4 jem_bay3)
+        (move-connected jem_bay4 jem_bay5)
+        (move-connected jem_bay5 jem_bay4)
+        (move-connected jem_bay5 jem_bay6)
+        (move-connected jem_bay6 jem_bay5)
+        (move-connected jem_bay6 jem_bay7)
+        (move-connected jem_bay7 jem_bay6)
+        (move-connected jem_bay7 jem_bay8)
+        (move-connected jem_bay8 jem_bay7)
+        (location-real jem_bay1)
+        (location-real jem_bay2)
+        (location-real jem_bay3)
+        (location-real jem_bay4)
+        (location-real jem_bay5)
+        (location-real jem_bay6)
+        (location-real jem_bay7)
+        (dock-connected jem_bay7 berth1)
+        (dock-connected jem_bay7 berth2)
+        (robots-different bumble honey)
+        (robots-different honey bumble)
+        (locations-different jem_bay0 jem_bay1)
+        (locations-different jem_bay0 jem_bay2)
+        (locations-different jem_bay0 jem_bay3)
+        (locations-different jem_bay0 jem_bay4)
+        (locations-different jem_bay0 jem_bay5)
+        (locations-different jem_bay0 jem_bay6)
+        (locations-different jem_bay0 jem_bay7)
+        (locations-different jem_bay0 jem_bay8)
+        (locations-different jem_bay1 jem_bay0)
+        (locations-different jem_bay1 jem_bay2)
+        (locations-different jem_bay1 jem_bay3)
+        (locations-different jem_bay1 jem_bay4)
+        (locations-different jem_bay1 jem_bay5)
+        (locations-different jem_bay1 jem_bay6)
+        (locations-different jem_bay1 jem_bay7)
+        (locations-different jem_bay1 jem_bay8)
+        (locations-different jem_bay2 jem_bay0)
+        (locations-different jem_bay2 jem_bay1)
+        (locations-different jem_bay2 jem_bay3)
+        (locations-different jem_bay2 jem_bay4)
+        (locations-different jem_bay2 jem_bay5)
+        (locations-different jem_bay2 jem_bay6)
+        (locations-different jem_bay2 jem_bay7)
+        (locations-different jem_bay2 jem_bay8)
+        (locations-different jem_bay3 jem_bay0)
+        (locations-different jem_bay3 jem_bay1)
+        (locations-different jem_bay3 jem_bay2)
+        (locations-different jem_bay3 jem_bay4)
+        (locations-different jem_bay3 jem_bay5)
+        (locations-different jem_bay3 jem_bay6)
+        (locations-different jem_bay3 jem_bay7)
+        (locations-different jem_bay3 jem_bay8)
+        (locations-different jem_bay4 jem_bay0)
+        (locations-different jem_bay4 jem_bay1)
+        (locations-different jem_bay4 jem_bay2)
+        (locations-different jem_bay4 jem_bay3)
+        (locations-different jem_bay4 jem_bay5)
+        (locations-different jem_bay4 jem_bay6)
+        (locations-different jem_bay4 jem_bay7)
+        (locations-different jem_bay4 jem_bay8)
+        (locations-different jem_bay5 jem_bay0)
+        (locations-different jem_bay5 jem_bay1)
+        (locations-different jem_bay5 jem_bay2)
+        (locations-different jem_bay5 jem_bay3)
+        (locations-different jem_bay5 jem_bay4)
+        (locations-different jem_bay5 jem_bay6)
+        (locations-different jem_bay5 jem_bay7)
+        (locations-different jem_bay5 jem_bay8)
+        (locations-different jem_bay6 jem_bay0)
+        (locations-different jem_bay6 jem_bay1)
+        (locations-different jem_bay6 jem_bay2)
+        (locations-different jem_bay6 jem_bay3)
+        (locations-different jem_bay6 jem_bay4)
+        (locations-different jem_bay6 jem_bay5)
+        (locations-different jem_bay6 jem_bay7)
+        (locations-different jem_bay6 jem_bay8)
+        (locations-different jem_bay7 jem_bay0)
+        (locations-different jem_bay7 jem_bay1)
+        (locations-different jem_bay7 jem_bay2)
+        (locations-different jem_bay7 jem_bay3)
+        (locations-different jem_bay7 jem_bay4)
+        (locations-different jem_bay7 jem_bay5)
+        (locations-different jem_bay7 jem_bay6)
+        (locations-different jem_bay7 jem_bay8)
+        (locations-different jem_bay8 jem_bay0)
+        (locations-different jem_bay8 jem_bay1)
+        (locations-different jem_bay8 jem_bay2)
+        (locations-different jem_bay8 jem_bay3)
+        (locations-different jem_bay8 jem_bay4)
+        (locations-different jem_bay8 jem_bay5)
+        (locations-different jem_bay8 jem_bay6)
+        (locations-different jem_bay8 jem_bay7)
+
+        ;; === Dynamic predicates ===
+        (robot-available bumble)
+        (robot-available honey)
+        (robot-at bumble berth1)
+        (robot-at honey berth2)
+        (location-available jem_bay0)
+        (location-available jem_bay1)
+        (location-available jem_bay2)
+        (location-available jem_bay3)
+        (location-available jem_bay4)
+        (location-available jem_bay5)
+        (location-available jem_bay6)
+        (location-available jem_bay7)
+        (location-available jem_bay8)
+        (completed-panorama bumble o0 jem_bay4)
+        (completed-panorama honey o1 jem_bay7)
+        (completed-stereo honey o4 jem_bay7 jem_bay4)
+        (need-stereo bumble o4 jem_bay1 jem_bay4)
+
+        ;; === Static numeric fluents ===
+        (= (order-identity o0) 0)
+        (= (order-identity o1) 1)
+        (= (order-identity o2) 2)
+        (= (order-identity o3) 3)
+        (= (order-identity o4) 4)
+
+        ;; === Dynamic numeric fluents ===
+        (= (robot-order bumble) -1)
+        (= (robot-order honey) -1)
+    )  ;; end :init
+)  ;; end problem

astrobee/survey_manager/survey_planner/data/test_cases/completed2_config.yaml

@@ -0,0 +1,43 @@
+# Copyright (c) 2023, United States Government, as represented by the
+# Administrator of the National Aeronautics and Space Administration.
+#
+# All rights reserved.
+#
+# The "ISAAC - Integrated System for Autonomous and Adaptive Caretaking
+# platform" software is 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.
+
+# Second test case for marking goals completed. This case is a copy of the baseline problem instance
+# to be used on ISS, but with the first few actions marked complete to simulate the system state if
+# a replan is needed partway through execution. Let's simulate the state at t=~1700 seconds when
+# each robot should have completed its first two panoramas.
+
+goals:
+
+- {type: panorama, robot: bumble, order: 0, location: jem_bay4, completed: true}
+- {type: panorama, robot: bumble, order: 1, location: jem_bay3, completed: true}
+- {type: panorama, robot: bumble, order: 2, location: jem_bay2}
+- {type: panorama, robot: bumble, order: 3, location: jem_bay1}
+- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3}
+- {type: robot_at, robot: bumble, location: berth1}
+
+- {type: panorama, robot: honey, order: 1, location: jem_bay7, completed: true}
+- {type: panorama, robot: honey, order: 2, location: jem_bay6, completed: true}
+- {type: panorama, robot: honey, order: 3, location: jem_bay5}
+- {type: stereo, robot: honey, order: 4, trajectory: jem_bay7_to_bay4}
+- {type: robot_at, robot: honey, location: berth2}
+
+init:
+  bumble:
+    location: jem_bay3
+  honey:
+    location: jem_bay6