Add proactive deconflicting heuristic to avoid planner crashes when r…

…obots need to move past each other (#137)

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

@@ -30,17 +30,21 @@ SURVEY_PLANNER_DIR=$(readlink -f "${THIS_DIR}/../..")
 PDDL_DIR="${SURVEY_PLANNER_DIR}/pddl"
 DATA_DIR="${SURVEY_PLANNER_DIR}/data"
 CASES_DIR="${DATA_DIR}/test_cases"
+DOMAIN="${PDDL_DIR}/domain_survey.pddl"
 cd "${THIS_DIR}"
 
-TEST_CASES=(completed1 completed2)
+TEST_CASES=(completed1 completed2 proactive1 proactive2 proactive3)
 
 set -x
 for i in "${TEST_CASES[@]}"; do
     { echo -e "\n=== $i ==="; } 2> /dev/null
-    rosrun survey_planner problem_generator.py "--config=${DATA_DIR}/survey_static.yaml,${CASES_DIR}/${i}_config.yaml" "--output=${CASES_DIR}/${i}_problem.pddl"
+    PROBLEM="${CASES_DIR}/${i}_problem.pddl"
+    PLAN="${CASES_DIR}/${i}_plan.pddl"
+    rosrun survey_planner problem_generator.py "--config=${DATA_DIR}/survey_static.yaml,${CASES_DIR}/${i}_config.yaml" "--output=${PROBLEM}"
     # Try to eliminate variation in problem.pddl due to embedding different local install paths in the output.
-    perl -i -ple "s:${SURVEY_PLANNER_DIR}/::g;" "${CASES_DIR}/${i}_problem.pddl"
-    rosrun survey_planner plan_survey.py "${PDDL_DIR}/domain_survey.pddl" "${CASES_DIR}/${i}_problem.pddl" > "${CASES_DIR}/${i}_plan.pddl"
-    { echo "[plan_survey.py stdout redirected to ${CASES_DIR}/${i}_plan.pddl]"; } 2> /dev/null
-    rosrun survey_planner plan_interpreter.py "--plan=${CASES_DIR}/${i}_plan.pddl" "--plot=${CASES_DIR}/${i}_plot.png" "--output=${CASES_DIR}/${i}_plan.yaml"
+    perl -i -ple "s:${SURVEY_PLANNER_DIR}/::g;" "${PROBLEM}"
+    rosrun survey_planner plan_survey.py "${DOMAIN}" "${PROBLEM}" > "${PLAN}"
+    { echo "[plan_survey.py stdout redirected to ${PLAN}]"; } 2> /dev/null
+    rosrun popf validate -t 0.0001 -e "${DOMAIN}" "${PROBLEM}" "${PLAN}"
+    rosrun survey_planner plan_interpreter.py "--plan=${PLAN}" "--plot=${CASES_DIR}/${i}_plot.png" "--output=${CASES_DIR}/${i}_plan.yaml"
 done

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'