Merge remote-tracking branch 'trey0/problem_gen_fixes' into survey_pl…

…anner

.github/workflows/ci_pr.yml

@@ -2,7 +2,7 @@ name: Build and Test CI for Pull Requests
 
 on:
   pull_request:
-    branches: [ 'master', 'develop' ]
+    branches: [ 'master', 'develop', 'survey_manager' ]
   workflow_dispatch:
 
 jobs:

.isort.cfg

@@ -15,4 +15,4 @@
 # folder, you can auto-update it by running
 # scripts/git/configure_isort_paths.sh.
 
-src_paths = analyst/workspace/scripts,anomaly/image/scripts,astrobee/behaviors/inspection/scripts,astrobee/simulation/acoustics_cam/src,dense_map/geometry_mapper/tools,isaac_msgs/isaac_msgs/test,pano/pano_stitch/scripts,pano/pano_view/scripts,scripts/git
+src_paths = analyst/workspace/scripts,anomaly/gmm-change-detection,anomaly/gmm-change-detection/scripts/gmm,anomaly/image/scripts,astrobee/behaviors/inspection/scripts,astrobee/simulation/acoustics_cam/src,astrobee/survey_manager/survey_planner/tools,dense_map/geometry_mapper/tools,dense_map/volumetric_mapper/scripts,pano/pano_stitch/scripts,pano/pano_view/scripts,scripts/git

anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py

@@ -23,6 +23,7 @@
 import sys
 
 import numpy as np
+
 from gmm.artificial_data import generate_data
 from gmm.gmm_edit import *
 from gmm.gmm_mml import GmmMml

astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml

@@ -24,11 +24,11 @@
 
 goals:
 
-- {type: panorama, robot: bumble, order: 0, location: jem_bay4, run: 1}
-- {type: panorama, robot: bumble, order: 1, location: jem_bay3, run: 1}
-- {type: panorama, robot: bumble, order: 2, location: jem_bay2, run: 1}
-- {type: panorama, robot: bumble, order: 3, location: jem_bay1, run: 1}
-- {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3, run: 1}
+- {type: panorama, robot: bumble, order: 0, location: jem_bay4}
+- {type: panorama, robot: bumble, order: 1, location: jem_bay3}
+- {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}
 
 # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to
 # get confused and greatly increase the total run time. For some reason, it doesn't notice it can
@@ -37,13 +37,13 @@ goals:
 # about 2x.
 # - {type: robot_at, robot: bumble, location: berth1}
 
-- {type: panorama, robot: honey, order: 0, location: jem_bay7, run: 1}
-- {type: panorama, robot: honey, order: 1, location: jem_bay6, run: 1}
-- {type: panorama, robot: honey, order: 2, location: jem_bay5, run: 1}
+- {type: panorama, robot: honey, order: 0, location: jem_bay7}
+- {type: panorama, robot: honey, order: 1, location: jem_bay6}
+- {type: panorama, robot: honey, order: 2, location: jem_bay5}
 
 # This is another objective we want to include that for some reason causes POPF to fail to generate
 # a plan (hang indefinitely). No obvious reason why it should cause a problem.
-# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7, run: 1}
+# - {type: stereo, robot: honey, order: 3, trajectory: jem_bay4_to_bay7}
 
 - {type: robot_at, robot: honey, location: berth2}
 

astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml

@@ -40,7 +40,6 @@ bays:
 bogus_bays: [jem_bay0, jem_bay8]
 berths: [berth1, berth2]
 robots: [bumble, honey]
-num_orders: 10
 
 stereo:
   # Meta-data about stereo survey options

astrobee/survey_manager/survey_planner/data/sample_output_plan.txt

@@ -1,31 +1,31 @@
-$ time popf domain_survey.pddl problem_jem_survey.pddl
+$ time popf pddl/domain_survey.pddl pddl/problem_jem_survey.pddl
 Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%]
 Post filtering unreachable actions:  [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%]
-3% of the ground temporal actions in this problem are compression-safe
-b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 990.004)b (20.000 | 990.004)b (19.000 | 1010.005)b (18.000 | 1910.006)b (17.000 | 1910.006)b (16.000 | 1930.007)b (15.000 | 2830.008)b (14.000 | 2830.008)b (13.000 | 2850.009)b (12.000 | 3750.010)b (11.000 | 3750.010)b (10.000 | 4350.011)b (9.000 | 4350.011)b (8.000 | 4350.011)b (7.000 | 4350.011)b (6.000 | 4350.011)b (5.000 | 4350.011)b (4.000 | 5270.013)b (3.000 | 5270.013)b (2.000 | 5290.014)b (1.000 | 5310.015);;;; Solution Found
-; Time 0.56
+16% of the ground temporal actions in this problem are compression-safe
+b (25.000 | 30.000)b (24.000 | 50.001)b (23.000 | 70.002)b (22.000 | 90.003)b (21.000 | 870.004)b (20.000 | 870.004)b (19.000 | 890.005)b (18.000 | 1670.006)b (17.000 | 1670.006)b (16.000 | 1690.007)b (15.000 | 2470.008)b (14.000 | 2470.008)b (13.000 | 2490.009)b (12.000 | 3270.010)b (11.000 | 3270.010)b (10.000 | 3870.011)b (9.000 | 3870.011)b (8.000 | 3870.011)b (7.000 | 3870.011)b (6.000 | 3870.011)b (5.000 | 3870.011)b (4.000 | 4670.013)b (3.000 | 4670.013)b (2.000 | 4690.014)b (1.000 | 4710.015);;;; Solution Found
+; Time 0.21
 0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6)  [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.003: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6)  [30.000]
-90.004: (panorama bumble o0 jem_bay4 run1)  [900.000]
-100.004: (panorama honey o0 jem_bay7 run1)  [900.000]
-990.005: (move bumble jem_bay4 jem_bay3 jem_bay2)  [20.000]
-1000.005: (move honey jem_bay7 jem_bay6 jem_bay5)  [20.000]
-1010.006: (panorama bumble o1 jem_bay3 run1)  [900.000]
-1020.006: (panorama honey o1 jem_bay6 run1)  [900.000]
-1910.007: (move bumble jem_bay3 jem_bay2 jem_bay1)  [20.000]
-1930.008: (panorama bumble o2 jem_bay2 run1)  [900.000]
-2830.009: (move bumble jem_bay2 jem_bay1 jem_bay0)  [20.000]
-2850.010: (panorama bumble o3 jem_bay1 run1)  [900.000]
-3750.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3 run1)  [600.000]
-4350.012: (move honey jem_bay6 jem_bay5 jem_bay4)  [20.000]
-4370.013: (panorama honey o2 jem_bay5 run1)  [900.000]
-5270.014: (move honey jem_bay5 jem_bay6 jem_bay7)  [20.000]
-5290.015: (move honey jem_bay6 jem_bay7 jem_bay8)  [20.000]
-5310.016: (dock honey jem_bay7 berth2)  [30.000]
+90.004: (panorama bumble o0 jem_bay4)  [780.000]
+100.004: (panorama honey o0 jem_bay7)  [780.000]
+870.005: (move bumble jem_bay4 jem_bay3 jem_bay2)  [20.000]
+880.005: (move honey jem_bay7 jem_bay6 jem_bay5)  [20.000]
+890.006: (panorama bumble o1 jem_bay3)  [780.000]
+900.006: (panorama honey o1 jem_bay6)  [780.000]
+1670.007: (move bumble jem_bay3 jem_bay2 jem_bay1)  [20.000]
+1690.008: (panorama bumble o2 jem_bay2)  [780.000]
+2470.009: (move bumble jem_bay2 jem_bay1 jem_bay0)  [20.000]
+2490.010: (panorama bumble o3 jem_bay1)  [780.000]
+3270.011: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3)  [600.000]
+3870.012: (move honey jem_bay6 jem_bay5 jem_bay4)  [20.000]
+3890.013: (panorama honey o2 jem_bay5)  [780.000]
+4670.014: (move honey jem_bay5 jem_bay6 jem_bay7)  [20.000]
+4690.015: (move honey jem_bay6 jem_bay7 jem_bay8)  [20.000]
+4710.016: (dock honey jem_bay7 berth2)  [30.000]
 
-real	0m0.585s
-user	0m0.553s
-sys	0m0.032s
+real	0m0.221s
+user	0m0.208s
+sys	0m0.013s

astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml

@@ -47,8 +47,7 @@
     - 11.0
     - -7.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
+  duration_seconds: '780.000'
 - start_time_seconds: '100.004'
   action:
     type: panorama
@@ -58,9 +57,8 @@
     - 11.0
     - -9.7
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '990.005'
+  duration_seconds: '780.000'
+- start_time_seconds: '870.005'
   action:
     type: move
     robot: bumble
@@ -70,7 +68,7 @@
     - -6.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '1000.005'
+- start_time_seconds: '880.005'
   action:
     type: move
     robot: honey
@@ -80,7 +78,7 @@
     - -9.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '1010.006'
+- start_time_seconds: '890.006'
   action:
     type: panorama
     robot: bumble
@@ -89,9 +87,8 @@
     - 11.0
     - -6.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '1020.006'
+  duration_seconds: '780.000'
+- start_time_seconds: '900.006'
   action:
     type: panorama
     robot: honey
@@ -100,9 +97,8 @@
     - 11.0
     - -9.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '1910.007'
+  duration_seconds: '780.000'
+- start_time_seconds: '1670.007'
   action:
     type: move
     robot: bumble
@@ -112,7 +108,7 @@
     - -5.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '1930.008'
+- start_time_seconds: '1690.008'
   action:
     type: panorama
     robot: bumble
@@ -121,9 +117,8 @@
     - 11.0
     - -5.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '2830.009'
+  duration_seconds: '780.000'
+- start_time_seconds: '2470.009'
   action:
     type: move
     robot: bumble
@@ -133,7 +128,7 @@
     - -4.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '2850.010'
+- start_time_seconds: '2490.010'
   action:
     type: panorama
     robot: bumble
@@ -142,16 +137,14 @@
     - 11.0
     - -4.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '3750.011'
+  duration_seconds: '780.000'
+- start_time_seconds: '3270.011'
   action:
     type: stereo
     robot: bumble
     fplan: jem_stereo_mapping_bay1_to_bay3.fplan
-    run: 1
   duration_seconds: '600.000'
-- start_time_seconds: '4350.012'
+- start_time_seconds: '3870.012'
   action:
     type: move
     robot: honey
@@ -161,7 +154,7 @@
     - -8.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '4370.013'
+- start_time_seconds: '3890.013'
   action:
     type: panorama
     robot: honey
@@ -170,9 +163,8 @@
     - 11.0
     - -8.0
     - 4.8
-    run: 1
-  duration_seconds: '900.000'
-- start_time_seconds: '5270.014'
+  duration_seconds: '780.000'
+- start_time_seconds: '4670.014'
   action:
     type: move
     robot: honey
@@ -182,7 +174,7 @@
     - -9.0
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '5290.015'
+- start_time_seconds: '4690.015'
   action:
     type: move
     robot: honey
@@ -192,7 +184,7 @@
     - -9.7
     - 4.8
   duration_seconds: '20.000'
-- start_time_seconds: '5310.016'
+- start_time_seconds: '4710.016'
   action:
     type: dock
     robot: honey

astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl

@@ -10,7 +10,6 @@
         location
         robot
         order
-        run-number
     )
 
     (:predicates
@@ -67,35 +66,31 @@
         ;; preconditions of the stereo action, and one of its effects is to clear the
         ;; predicate. Therefore, the planner won't waste time trying to execute stereo actions that
         ;; the user didn't explicitly request. Without this hack, the planner run time blows up.
-        (need-stereo ?robot - robot ?order - order ?base ?bound - location ?run-number - run-number)
+        (need-stereo ?robot - robot ?order - order ?base ?bound - location)
 
         ;; === Goal predicates ===
         ;; completed-panorama: The goal to add if you want the plan to include collecting a
         ;; panorama. For now, goals specify ?robot and ?order parameters that constrain
-        ;; multi-robot task allocation and task ordering. The ?run-number is used to indicate
-        ;; retries and is meaningless to the planner but helpful for post-run analysis.
+        ;; multi-robot task allocation and task ordering.
         (completed-panorama
             ?robot - robot
             ?order - order
             ?location - location
-            ?run-number - run-number
         )
 
         ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo
         ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot
-        ;; task allocation and task ordering. The ?run-number is used to indicate retries and is
-        ;; meaningless to the planner but helpful for post-run analysis. The current model for
-        ;; stereo surveys assumes the robot starts and ends the survey at the same location called
-        ;; ?base (these locations only need to be same to the effective precision modeled in the
-        ;; planner, "less than a bay apart").  The ?bound argument indicates the other end of the
-        ;; interval covered by the survey and is used for collision checking. It's assumed that
-        ;; ?base and ?bound are not adjacent locations. If future stereo surveys violate these
-        ;; assumptions the model will need to be revisited.
+        ;; task allocation and task ordering. The current model for stereo surveys assumes the robot
+        ;; starts and ends the survey at the same location called ?base (these locations only need
+        ;; to be same to the effective precision modeled in the planner, "less than a bay apart").
+        ;; The ?bound argument indicates the other end of the interval covered by the survey and is
+        ;; used for collision checking. It's assumed that ?base and ?bound are not adjacent
+        ;; locations. If future stereo surveys violate these assumptions the model will need to be
+        ;; revisited.
         (completed-stereo
             ?robot - robot
             ?order - order
             ?base ?bound - location
-            ?run-number - run-number
         )
     )
 
@@ -237,7 +232,6 @@
                 ?robot - robot
                 ?order - order
                 ?location - location
-                ?run-number - run-number
             )
         ;; ~13 minutes, per https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure
         :duration (= ?duration 780)
@@ -262,7 +256,7 @@
                 (at end (assign (robot-order ?robot) (order-identity ?order)))
 
                 ;; Mark success
-                (at end (completed-panorama ?robot ?order ?location ?run-number))
+                (at end (completed-panorama ?robot ?order ?location))
             )
     )
 
@@ -275,7 +269,6 @@
                 ?base ?bound - location
                 ;; ?check1 and ?check2: Planner-selected neighbors of ?bound for collision check
                 ?check1 ?check2 - location
-                ?run-number - run-number
             )
         :duration (= ?duration 600)  ;; 10 minutes
         :condition
@@ -292,7 +285,7 @@
 
                 ;; Check for need-stereo so the planner only tries this action when the user
                 ;; explicitly requests it.
-                (at start (need-stereo ?robot ?order ?base ?bound ?run-number))
+                (at start (need-stereo ?robot ?order ?base ?bound))
 
                 ;; Check collision avoidance
                 (at start (location-available ?bound))
@@ -321,10 +314,10 @@
 
                 ;; Clear need-stereo so the planner won't try to use the stereo action
                 ;; again after the user request is satisfied.
-                (at end (not (need-stereo ?robot ?order ?base ?bound ?run-number)))
+                (at end (not (need-stereo ?robot ?order ?base ?bound)))
 
                 ;; Mark success
-                (at end (completed-stereo ?robot ?order ?base ?bound ?run-number))
+                (at end (completed-stereo ?robot ?order ?base ?bound))
             )
     )