Survey planner: respect when problem instance specifies actions were …

…completed

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: /home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/src/survey_planner/problem_generator.py --config=/home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/data/survey_static.yaml,/home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/data/test_cases/completed1_config.yaml --output=/home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/data/test_cases/completed1_problem.pddl
+;; Working directory was: /home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/data/test_cases
+;; Problem template: ../../pddl/jem_survey_template.pddl
+;; Config 1: /home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/data/survey_static.yaml
+;; Config 2: /home/vagrant/isaac/src/astrobee/survey_manager/survey_planner/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

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

@@ -0,0 +1,20 @@
+; Solution Found
+0.000: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000]
+0.001: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000]
+20.001: (panorama bumble o2 jem_bay2) [780.000]
+20.002: (panorama honey o3 jem_bay5) [780.000]
+800.002: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000]
+800.003: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000]
+820.003: (panorama bumble o3 jem_bay1) [780.000]
+820.004: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000]
+840.005: (stereo honey o4 jem_bay7 jem_bay4 jem_bay3 jem_bay5) [600.000]
+1440.006: (dock honey jem_bay7 berth2) [30.000]
+1600.004: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) [600.000]
+2200.005: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000]
+2220.006: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000]
+2240.007: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000]
+2260.008: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000]
+2280.009: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000]
+2300.010: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000]
+2320.011: (dock bumble jem_bay7 berth1) [30.000]
+