Add proactive deconflicting heuristic to avoid planner crashes when r…

…obots need to move past each other

astrobee/survey_manager/survey_planner/data/test_cases/generate.bash

@@ -32,7 +32,7 @@ DATA_DIR="${SURVEY_PLANNER_DIR}/data"
 CASES_DIR="${DATA_DIR}/test_cases"
 cd "${THIS_DIR}"
 
-TEST_CASES=(completed1 completed2)
+TEST_CASES=(completed1 completed2 proactive1 proactive2)
 
 set -x
 for i in "${TEST_CASES[@]}"; do

astrobee/survey_manager/survey_planner/data/test_cases/proactive1_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.
+
+# Test case for a robot proactively getting out of another robot's way. The first robot mentioned
+# in the 'goals' section is considered the higher priority robot and the other robot should get
+# out of its way in a conflict. In this problem, Bumble is the higher priority robot and it needs
+# to go through Honey's current location to get to its desired pano location, so Honey should
+# move to get out of its way.
+
+goals:
+
+# Note: The proactive move-out-of-the-way heuristic will fail causing a planner crash if Bumble's
+# pano is requested in jem_bay7 because Honey arbitrarily chooses to get out of Bumble's way by
+# moving to berth1, then Honey has nowhere to go to get out of Bumble's way when Bumble tries to
+# move to berth1 afterward. We will probably not be able to fix that kind of case robustly because
+# it would require a major overhaul of the deconflicting approach, making things considerably more
+# complicated. Luckily, it seems unlikely to arise in practice.
+
+- {type: panorama, robot: bumble, order: 0, location: jem_bay5}
+- {type: robot_at, robot: bumble, location: berth1}
+
+- {type: panorama, robot: honey, order: 0, location: jem_bay1}
+
+init:
+  bumble:
+    location: jem_bay3
+  honey:
+    location: jem_bay5

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

@@ -0,0 +1,19 @@
+; Solution Found
+0.000: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000]
+20.001: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000]
+20.002: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000]
+40.003: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000]
+60.004: (panorama bumble o0 jem_bay5) [780.000]
+840.005: (dock honey jem_bay7 berth2) [30.000]
+870.006: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000]
+890.007: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000]
+910.008: (dock bumble jem_bay7 berth1) [30.000]
+940.009: (undock honey berth2 jem_bay7 jem_bay6 jem_bay8) [30.000]
+970.010: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000]
+990.011: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000]
+1010.012: (move honey jem_bay5 jem_bay4 jem_bay3) [20.000]
+1030.013: (move honey jem_bay4 jem_bay3 jem_bay2) [20.000]
+1050.014: (move honey jem_bay3 jem_bay2 jem_bay1) [20.000]
+1070.015: (move honey jem_bay2 jem_bay1 jem_bay0) [20.000]
+1090.016: (panorama honey o0 jem_bay1) [780.000]
+

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

@@ -0,0 +1,169 @@
+- start_time_seconds: '0.000'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay5
+    to_name: jem_bay6
+    to_pos:
+    - 11.0
+    - -9.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '20.001'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay3
+    to_name: jem_bay4
+    to_pos:
+    - 11.0
+    - -7.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '20.002'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay6
+    to_name: jem_bay7
+    to_pos:
+    - 11.0
+    - -9.7
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '40.003'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay4
+    to_name: jem_bay5
+    to_pos:
+    - 11.0
+    - -8.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '60.004'
+  action:
+    type: panorama
+    robot: bumble
+    location_name: jem_bay5
+    location_pos:
+    - 11.0
+    - -8.0
+    - 4.8
+  duration_seconds: '780.000'
+- start_time_seconds: '840.005'
+  action:
+    type: dock
+    robot: honey
+    berth: berth2
+  duration_seconds: '30.000'
+- start_time_seconds: '870.006'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay5
+    to_name: jem_bay6
+    to_pos:
+    - 11.0
+    - -9.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '890.007'
+  action:
+    type: move
+    robot: bumble
+    from_name: jem_bay6
+    to_name: jem_bay7
+    to_pos:
+    - 11.0
+    - -9.7
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '910.008'
+  action:
+    type: dock
+    robot: bumble
+    berth: berth1
+  duration_seconds: '30.000'
+- start_time_seconds: '940.009'
+  action:
+    type: undock
+    robot: honey
+  duration_seconds: '30.000'
+- start_time_seconds: '970.010'
+  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: '990.011'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay6
+    to_name: jem_bay5
+    to_pos:
+    - 11.0
+    - -8.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '1010.012'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay5
+    to_name: jem_bay4
+    to_pos:
+    - 11.0
+    - -7.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '1030.013'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay4
+    to_name: jem_bay3
+    to_pos:
+    - 11.0
+    - -6.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '1050.014'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay3
+    to_name: jem_bay2
+    to_pos:
+    - 11.0
+    - -5.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '1070.015'
+  action:
+    type: move
+    robot: honey
+    from_name: jem_bay2
+    to_name: jem_bay1
+    to_pos:
+    - 11.0
+    - -4.0
+    - 4.8
+  duration_seconds: '20.000'
+- start_time_seconds: '1090.016'
+  action:
+    type: panorama
+    robot: honey
+    location_name: jem_bay1
+    location_pos:
+    - 11.0
+    - -4.0
+    - 4.8
+  duration_seconds: '780.000'

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

@@ -0,0 +1,149 @@
+;; 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/proactive1_config.yaml --output=data/test_cases/proactive1_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/proactive1_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 - order
+    )
+
+    (:goal
+        (and
+            (completed-panorama bumble o0 jem_bay5)
+            (robot-at bumble berth1)
+            (completed-panorama honey o0 jem_bay1)
+        )
+    )
+
+    (: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 jem_bay3)
+        (robot-at honey jem_bay5)
+        (location-available berth1)
+        (location-available berth2)
+        (location-available jem_bay0)
+        (location-available jem_bay1)
+        (location-available jem_bay2)
+        (location-available jem_bay4)
+        (location-available jem_bay6)
+        (location-available jem_bay7)
+        (location-available jem_bay8)
+
+        ;; === Static numeric fluents ===
+        (= (order-identity o0) 0)
+
+        ;; === Dynamic numeric fluents ===
+        (= (robot-order bumble) -1)
+        (= (robot-order honey) -1)
+    )  ;; end :init
+)  ;; end problem