From 7ac684573104aaf500f54d2825d69506947d7d20 Mon Sep 17 00:00:00 2001
From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com>
Date: Fri, 10 May 2024 09:46:41 -0700
Subject: [PATCH] ISAAC 15 prep (#164)

* isaac 15 in usl + fixes to command_astrobee + allowing for exposure and map changes

* update submodule

* fixing static poses + optimizing change exposure and map changes

* updates for isaac15 + granite lab testing with exposure and map change

* switch berths on problem generator

* fix gp install
---
 .../simulation/isaac_gazebo/CMakeLists.txt    |   4 +-
 .../survey_dependencies/ros2_planning_system  |   2 +-
 .../data/granite_survey_dynamic.yaml          |  16 +-
 .../data/granite_survey_static.yaml           |  57 +-
 ...vey_static.yaml => iss_survey_static.yaml} |  52 +-
 .../data/jem_survey_dynamic.yaml              |   8 +-
 .../data/usl_survey_dynamic.yaml              |  53 ++
 .../pddl/problem_granite_survey.ps2.pddl      | 145 ++++-
 .../pddl/problem_usl_survey.ps2.pddl          | 594 ++++++++++++++++++
 .../src/survey_manager/command_astrobee.py    | 170 +++--
 .../src/survey_manager/problem_generator.py   |   6 +-
 dense_map/volumetric_mapper/CMakeLists.txt    |   2 +-
 .../setup/dependencies/build_install_gp.sh    |   2 +-
 13 files changed, 991 insertions(+), 120 deletions(-)
 rename astrobee/survey/survey_manager/data/{jem_survey_static.yaml => iss_survey_static.yaml} (76%)
 create mode 100644 astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml
 create mode 100644 astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl

diff --git a/astrobee/simulation/isaac_gazebo/CMakeLists.txt b/astrobee/simulation/isaac_gazebo/CMakeLists.txt
index cf5e0ad6..c3a72b0a 100644
--- a/astrobee/simulation/isaac_gazebo/CMakeLists.txt
+++ b/astrobee/simulation/isaac_gazebo/CMakeLists.txt
@@ -19,8 +19,8 @@
 cmake_minimum_required(VERSION 3.5)
 project(isaac_gazebo)
 
-# Specify C++14 standard
-add_definitions(-std=c++14)
+# Specify C++17 standard
+add_definitions(-std=c++17)
 
 # Find packages
 find_package(catkin REQUIRED COMPONENTS
diff --git a/astrobee/survey/survey_dependencies/ros2_planning_system b/astrobee/survey/survey_dependencies/ros2_planning_system
index 50e8210d..62adf29e 160000
--- a/astrobee/survey/survey_dependencies/ros2_planning_system
+++ b/astrobee/survey/survey_dependencies/ros2_planning_system
@@ -1 +1 @@
-Subproject commit 50e8210d09d6cd6877406aabe0fbcb70d6b67ab0
+Subproject commit 62adf29e4531668ab11c8ead75c82808e9f071e1
diff --git a/astrobee/survey/survey_manager/data/granite_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/granite_survey_dynamic.yaml
index 65dc0846..50fa806e 100644
--- a/astrobee/survey/survey_manager/data/granite_survey_dynamic.yaml
+++ b/astrobee/survey/survey_manager/data/granite_survey_dynamic.yaml
@@ -24,11 +24,11 @@
 
 goals:
 
-- {type: panorama, robot: bsharp, order: 0, location: gra_bay2}
-- {type: stereo, robot: bsharp, order: 1, trajectory: gra_bay1_to_bay3}
+- {type: panorama, robot: bsharp, order: 0, location: gra_bay6}
+- {type: stereo, robot: bsharp, order: 1, trajectory: gro_bay1_to_bay3}
 # This is one of the goals we previously had to comment out for POPF to return a halfway decent
 # plan. Adding a let_other_robot_reach goal mostly fixed the problem.
-- {type: robot_at, robot: bsharp, location: berth1}
+- {type: robot_at, robot: bsharp, location: berth1_g}
 
 # This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering
 # constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without
@@ -37,13 +37,13 @@ goals:
 # planner doesn't need the hint.)
 # - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5}
 
-- {type: panorama, robot: wannabee, order: 1, location: gra_bay6}
+- {type: panorama, robot: wannabee, order: 1, location: gra_bay2}
 # This is the other objective we previously had to comment out for POPF to return a decent plan.
-- {type: stereo, robot: wannabee, order: 4, trajectory: gra_bay5_to_bay7}
-- {type: robot_at, robot: wannabee, location: berth2}
+- {type: stereo, robot: wannabee, order: 4, trajectory: gra_bay1_to_bay3}
+- {type: robot_at, robot: wannabee, location: berth2_g}
 
 init:
   bsharp:
-    location: berth1
+    location: berth1_g
   wannabee:
-    location: berth2
+    location: berth2_g
diff --git a/astrobee/survey/survey_manager/data/granite_survey_static.yaml b/astrobee/survey/survey_manager/data/granite_survey_static.yaml
index 675e9c25..7deb0dc6 100644
--- a/astrobee/survey/survey_manager/data/granite_survey_static.yaml
+++ b/astrobee/survey/survey_manager/data/granite_survey_static.yaml
@@ -27,6 +27,18 @@
 # Useful reference for positions and stereo survey trajectories:
 # https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure
 
+# granite map:
+#
+#           gra_1
+#           gra_2
+#   berth2| gra_3
+#
+#   berth1| gra_4
+#           gra_5
+#           gra_6
+#           gra_7  gro_0  gro_1  gro_2  gro_3
+#
+
 bays:
   # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee
   gra_bay1: [0.1, 0.7, -0.68]
@@ -35,30 +47,47 @@ bays:
   gra_bay4: [0.1, 0.3, -0.68]
   gra_bay5: [0.1, 0.1, -0.68]
   gra_bay6: [0.1, -0.3, -0.68]
-  gra_bay7: [0.1, -0.5, -0.68]
+  gra_bay7: [0.15, -0.5, -0.68]
+  gra_bay8: [0.15, -0.5, -0.68]
+  gro_bay0: [0.20, -0.5, -0.68]
+  gro_bay1: [0.25, -0.5, -0.68]
+  gro_bay2: [0.30, -0.5, -0.68]
+  gro_bay3: [0.35, -0.5, -0.68]
 
 bays_move:
-  gra_bay1: ["-pos", "0.1 0.7 -0.68"]
-  gra_bay2: ["-pos", "0.1 0.6 -0.68"]
-  gra_bay3: ["-pos", "0.1 0.5 -0.68"]
+  gra_bay1: ["-pos", "0.1 -0.5 -0.68", "-att", "3.14 1 0 0"]
+  gra_bay2: ["-pos", "0.1 -0.4 -0.68", "-att", "3.14 1 0 0"]
+  gra_bay3: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
   gra_bay4: ["-pos", "0.1 0.3 -0.68"]
-  gra_bay5: ["-pos", "0.1 0.1 -0.68", "-att", "3.14 1 0 0"]
-  gra_bay6: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
-  gra_bay7: ["-pos", "0.1 -0.5 -0.68", "-att", "3.14 1 0 0"]
+  gra_bay5: ["-pos", "0.1 0.4 -0.68"]
+  gra_bay6: ["-pos", "0.1 0.5 -0.68"]
+  gra_bay7: ["-pos", "0.15 0.7 -0.68"]
+  gra_bay8: ["-pos", "0.20 0.7 -0.68"]
+  gro_bay0: ["-pos", "0.25 0.7 -0.68"]
+  gro_bay1: ["-pos", "0.30 0.7 -0.68"]
+  gro_bay2: ["-pos", "0.35 0.7 -0.68"]
+  gro_bay3: ["-pos", "0.40 0.7 -0.68"]
   berth1_g: ["-pos", "0.1 0.3 -0.68"]
   berth2_g: ["-pos", "0.1 -0.3 -0.68", "-att", "3.14 1 0 0"]
 
+maps:
+  gra: "/data/maps/granite.map"
+  gro: "/data/maps/iss.map"
+
+exposure:
+  gra: 175
+  gro: 300
 
 bays_pano:
-  gra_bay2: "panorama_granite_bsharp.txt"
-  gra_bay6: "panorama_granite_wannabee.txt"
+  gra_bay6: "panorama_granite_bsharp.txt"
+  gra_bay2: "panorama_granite_wannabee.txt"
 
 berth:
   berth1_g: "1"
   berth2_g: "2"
 
-bogus_bays: [gra_bay0, gra_bay8]
-berths: [berth1, berth2]
+bogus_bays: [gra_bay0, gra_bay8, gro_bay0, gro_bay4]
+berths: [berth1_g, berth2_g]
 robots: [bsharp, wannabee]
 
 stereo:
@@ -66,7 +95,7 @@ stereo:
     fplan: "startup"
     base_location: gra_bay1
     bound_location: gra_bay3
-  gra_bay5_to_bay7:
+  gro_bay1_to_bay3:
     fplan: "startup"
-    base_location: gra_bay5
-    bound_location: gra_bay7
+    base_location: gro_bay1
+    bound_location: gro_bay3
diff --git a/astrobee/survey/survey_manager/data/jem_survey_static.yaml b/astrobee/survey/survey_manager/data/iss_survey_static.yaml
similarity index 76%
rename from astrobee/survey/survey_manager/data/jem_survey_static.yaml
rename to astrobee/survey/survey_manager/data/iss_survey_static.yaml
index 255edf9d..8627f075 100644
--- a/astrobee/survey/survey_manager/data/jem_survey_static.yaml
+++ b/astrobee/survey/survey_manager/data/iss_survey_static.yaml
@@ -36,6 +36,19 @@ bays:
   jem_bay5: [11.0, -8.0, 4.8]
   jem_bay6: [11.0, -9.0, 4.8]
   jem_bay7: [11.0, -9.7, 4.8]
+  jem_bay0: [11.0, -9.7, 4.8]
+  nod2_bay1: [11.0, -9.7, 4.8]
+  nod2_bay2: [11.0, -1.0, 4.8]
+  nod2_bay3: [10.0, 0.0, 4.8]
+  nod2_bay4: [9.0, 0.0, 4.8]
+  nod2_bay5: [7.8, 0.0, 4.8]
+  usl_bay0: [5.3, 0.0, 4.8]
+  usl_bay1: [4.7, 0.0, 4.8]
+  usl_bay2: [3.65, 0.0, 4.8]
+  usl_bay3: [2.6, 0.0, 4.8]
+  usl_bay4: [1.55, 0.0, 4.8]
+  usl_bay5: [0.5, 0.0, 4.8]
+  usl_bay6: [-0.5, 0.0, 4.8]
 
 bays_move:
   jem_bay1: ["-pos", "11 -4 4.8", "-att", "1.5708 0 0 1"]
@@ -45,23 +58,19 @@ bays_move:
   jem_bay5: ["-pos", "11 -8 4.8", "-att", "-1.5708 0 0 1"]
   jem_bay6: ["-pos", "11 -9 4.8", "-att", "-1.5708 0 0 1"]
   jem_bay7: ["-pos", "11 -9.7 4.8", "-att", "-1.5708 0 0 1"]
-  jem_hatch_to_nod2: ["-pos", "11 -3.5 4.8", "-att", "1.5708 0 0 1"]
-  jem_hatch_from_nod2: ["-pos", "11 -3.5 4.8", "-att", "-1.5708 0 0 1"]
-  nod2_hatch_from_jem: ["-pos", "11 -1.0 4.8", "-att", "1.5708 0 0 1"]
-  nod2_hatch_to_jem: ["-pos", "11 -1.0 4.8", "-att", "-1.5708 0 0 1"]
+  jem_bay0: ["-pos", "11 -3.5 4.8", "-att", "-1.5708 0 0 1"]
+  nod2_bay1: ["-pos", "11 -1.0 4.8", "-att", "-1.5708 0 0 1"]
   nod2_bay2: ["-pos", "11 0 4.8", "-att", "0 0 0 1"]
   nod2_bay3: ["-pos", "10 0 4.8", "-att", "0 0 0 1"]
   nod2_bay4: ["-pos", "9 0 4.8", "-att", "0 0 0 1"]
-  nod2_hatch_to_usl: ["-pos", "7.8 -3.5 4.8", "-att", "3.14 0 0 1"]
-  nod2_hatch_from_usl: ["-pos", "7.8 -3.5 4.8", "-att", "0 0 0 1"]
-  usl_hatch_from_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "3.14 0 0 1"]
-  usl_hatch_to_nod2: ["-pos", "5.3 -1.0 4.8", "-att", "0 0 0 1"]
+  nod2_bay5: ["-pos", "7.8 0.0 4.8", "-att", "0 0 0 1"]
+  usl_bay0: ["-pos", "5.3 0.0 4.8", "-att", "0 0 0 1"]
   usl_bay1: ["-pos", "4.7 0 4.8", "-att", "0 0 0 1"]
   usl_bay2: ["-pos", "3.65 0 4.8", "-att", "0 0 0 1"]
   usl_bay3: ["-pos", "2.6 0 4.8", "-att", "0 0 0 1"]
-  usl_bay4: ["-pos", "1.55 0 4.8", "-att", "0 0 0 1"]
-  usl_bay5: ["-pos", "0.5 0 4.8", "-att", "0 0 0 1"]
-  usl_bay6: ["-pos", "-0.5 0 4.8", "-att", "0 0 0 1"]
+  usl_bay4: ["-pos", "1.55 0 4.8", "-att", "3.14 0 0 1"]
+  usl_bay5: ["-pos", "0.5 0 4.8", "-att", "3.14 0 0 1"]
+  usl_bay6: ["-pos", "-0.5 0 4.8", "-att", "3.14 0 0 1"]
   # berth approach points from gds_configs/IssWorld/BookmarksList.json
   berth1: ["-pos", "10.5 -9.25 4.5", "-att", "0 0 0 1"]
   berth2: ["-pos", "10.5 -9.75 4.5", "-att", "3.14 1 0 0"]
@@ -85,19 +94,20 @@ bays_pano:
   usl_bay6: "isaac11/usl_bay6_std_panorama.txt"
 
 maps:
-  jem: "iss.map"
-  nod2: "isaac.map"
-  usl: "usl_only.map"
+  jem: "/data/maps/20240320_lam.map"
+  nod2: "/data/maps/20240205_usl_abad.map"
+  usl: "/data/maps/20240205_usl_abad.map"
 
 exposure:
   jem: 175
   nod2: 300
   usl: 300
+
 berth:
   berth1: "1"
   berth2: "2"
 
-bogus_bays: [jem_bay0, jem_bay8]
+bogus_bays: [jem_bay0, jem_bay8, nod2_bay1, nod2_bay5, usl_bay0, usl_bay7]
 berths: [berth1, berth2]
 robots: [bumble, honey]
 
@@ -116,3 +126,15 @@ stereo:
     fplan: "ISAAC/jem_stereo_mapping_bay7_to_bay4"
     base_location: jem_bay7
     bound_location: jem_bay4
+  usl_bay1_to_bay3:
+    fplan: "ISAAC/usl_stereo_mapping_bay1_to_bay3"
+    base_location: usl_bay1
+    bound_location: usl_bay3
+  usl_bay6_to_bay4:
+    fplan: "ISAAC/usl_stereo_mapping_bay6_to_bay4"
+    base_location: usl_bay6
+    bound_location: usl_bay4
+  usl_bay1_to_bay6:
+    fplan: "ISAAC/usl_stereo_mapping_start_bay1"
+    base_location: usl_bay1
+    bound_location: usl_bay6
diff --git a/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml
index 841764d9..69e8d29a 100644
--- a/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml
+++ b/astrobee/survey/survey_manager/data/jem_survey_dynamic.yaml
@@ -31,7 +31,7 @@ goals:
 - {type: stereo, robot: bumble, order: 4, trajectory: jem_bay1_to_bay3}
 # This is one of the goals we previously had to comment out for POPF to return a halfway decent
 # plan. Adding a let_other_robot_reach goal mostly fixed the problem.
-- {type: robot_at, robot: bumble, location: berth1_g}
+- {type: robot_at, robot: bumble, location: berth1}
 
 # This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering
 # constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without
@@ -45,10 +45,10 @@ goals:
 - {type: panorama, robot: honey, order: 3, location: jem_bay5}
 # This is the other objective we previously had to comment out for POPF to return a decent plan.
 - {type: stereo, robot: honey, order: 4, trajectory: jem_bay7_to_bay4}
-- {type: robot_at, robot: honey, location: berth2_g}
+- {type: robot_at, robot: honey, location: berth2}
 
 init:
   bumble:
-    location: berth1_g
+    location: berth1
   honey:
-    location: berth2_g
+    location: berth2
diff --git a/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml b/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml
new file mode 100644
index 00000000..86466314
--- /dev/null
+++ b/astrobee/survey/survey_manager/data/usl_survey_dynamic.yaml
@@ -0,0 +1,53 @@
+# 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.
+
+# Example dynamic configuration info used when generating a PDDL problem.  For now, this is goal
+# conditions and initial state. A likely conops is that the initial version of this file for a
+# specific activity would be hand-generated, but it might later be automatically regenerated by the
+# survey manager when a replan is needed (remove completed/failed goals, add retry goals, update
+# initial state to match actual current state, etc.)  See also jem_survey_static.yaml.
+
+goals:
+
+- {type: panorama, robot: bumble, order: 1, location: usl_bay3}
+- {type: panorama, robot: bumble, order: 2, location: usl_bay2}
+- {type: panorama, robot: bumble, order: 3, location: usl_bay1}
+# - {type: stereo, robot: bumble, order: 4, trajectory: usl_bay1_to_bay3}
+# This is one of the goals we previously had to comment out for POPF to return a halfway decent
+# plan. Adding a let_other_robot_reach goal mostly fixed the problem.
+- {type: robot_at, robot: bumble, location: berth1}
+
+# This let_other_robot_reach goal is effectively a very specific kind of between-robot ordering
+# constraint. It tells honey to let bumble get to bay 5 before taking its first panorama. Without
+# this constraint, POPF produces a very inefficient plan where bumble never leaves the dock until
+# after honey finishes all its tasks and returns to dock. (It's safe to comment this out if the
+# planner doesn't need the hint.)
+# - {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5}
+
+- {type: panorama, robot: honey, order: 1, location: usl_bay6}
+- {type: panorama, robot: honey, order: 2, location: usl_bay5}
+- {type: panorama, robot: honey, order: 3, location: usl_bay4}
+# This is the other objective we previously had to comment out for POPF to return a decent plan.
+- {type: stereo, robot: honey, order: 4, trajectory: usl_bay1_to_bay6}
+- {type: robot_at, robot: honey, location: berth2}
+
+init:
+  bumble:
+    location: usl_bay1
+  honey:
+    location: usl_bay6
diff --git a/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl b/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl
index 1575bf06..87160a7c 100644
--- a/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl
+++ b/astrobee/survey/survey_manager/pddl/problem_granite_survey.ps2.pddl
@@ -7,6 +7,11 @@ set instance gra_bay5 location
 set instance gra_bay6 location
 set instance gra_bay7 location
 set instance gra_bay8 location
+set instance gro_bay0 location
+set instance gro_bay1 location
+set instance gro_bay2 location
+set instance gro_bay3 location
+set instance gro_bay4 location
 set instance berth1_g location
 set instance berth2_g location
 set instance bsharp robot
@@ -16,7 +21,7 @@ set instance o1 order
 set instance o2 order
 set instance o3 order
 set instance o4 order
-set goal (and (completed-panorama bsharp o0 gra_bay2) (completed-stereo bsharp o1 gra_bay1 gra_bay3) (robot-at bsharp berth1_g) (completed-panorama wannabee o1 gra_bay6) (completed-stereo wannabee o4 gra_bay5 gra_bay7) (robot-at wannabee berth2_g))
+set goal (and (completed-panorama bsharp o0 gra_bay6) (completed-stereo bsharp o1 gro_bay1 gro_bay3) (robot-at bsharp berth1_g) (completed-panorama wannabee o1 gra_bay2) (completed-stereo wannabee o4 gra_bay1 gra_bay3) (robot-at wannabee berth2_g))
 set predicate (move-connected gra_bay0 gra_bay1)
 set predicate (move-connected gra_bay1 gra_bay0)
 set predicate (move-connected gra_bay1 gra_bay2)
@@ -33,6 +38,16 @@ set predicate (move-connected gra_bay6 gra_bay7)
 set predicate (move-connected gra_bay7 gra_bay6)
 set predicate (move-connected gra_bay7 gra_bay8)
 set predicate (move-connected gra_bay8 gra_bay7)
+set predicate (move-connected gra_bay8 gro_bay0)
+set predicate (move-connected gro_bay0 gra_bay8)
+set predicate (move-connected gro_bay0 gro_bay1)
+set predicate (move-connected gro_bay1 gro_bay0)
+set predicate (move-connected gro_bay1 gro_bay2)
+set predicate (move-connected gro_bay2 gro_bay1)
+set predicate (move-connected gro_bay2 gro_bay3)
+set predicate (move-connected gro_bay3 gro_bay2)
+set predicate (move-connected gro_bay3 gro_bay4)
+set predicate (move-connected gro_bay4 gro_bay3)
 set predicate (location-real gra_bay1)
 set predicate (location-real gra_bay2)
 set predicate (location-real gra_bay3)
@@ -40,8 +55,13 @@ set predicate (location-real gra_bay4)
 set predicate (location-real gra_bay5)
 set predicate (location-real gra_bay6)
 set predicate (location-real gra_bay7)
-set predicate (dock-connected gra_bay3 berth1_g)
-set predicate (dock-connected gra_bay5 berth2_g)
+set predicate (location-real gra_bay8)
+set predicate (location-real gro_bay0)
+set predicate (location-real gro_bay1)
+set predicate (location-real gro_bay2)
+set predicate (location-real gro_bay3)
+set predicate (dock-connected gra_bay4 berth1_g)
+set predicate (dock-connected gra_bay3 berth2_g)
 set predicate (robots-different bsharp wannabee)
 set predicate (robots-different wannabee bsharp)
 set predicate (locations-different gra_bay0 gra_bay1)
@@ -52,6 +72,11 @@ set predicate (locations-different gra_bay0 gra_bay5)
 set predicate (locations-different gra_bay0 gra_bay6)
 set predicate (locations-different gra_bay0 gra_bay7)
 set predicate (locations-different gra_bay0 gra_bay8)
+set predicate (locations-different gra_bay0 gro_bay0)
+set predicate (locations-different gra_bay0 gro_bay1)
+set predicate (locations-different gra_bay0 gro_bay2)
+set predicate (locations-different gra_bay0 gro_bay3)
+set predicate (locations-different gra_bay0 gro_bay4)
 set predicate (locations-different gra_bay1 gra_bay0)
 set predicate (locations-different gra_bay1 gra_bay2)
 set predicate (locations-different gra_bay1 gra_bay3)
@@ -60,6 +85,11 @@ set predicate (locations-different gra_bay1 gra_bay5)
 set predicate (locations-different gra_bay1 gra_bay6)
 set predicate (locations-different gra_bay1 gra_bay7)
 set predicate (locations-different gra_bay1 gra_bay8)
+set predicate (locations-different gra_bay1 gro_bay0)
+set predicate (locations-different gra_bay1 gro_bay1)
+set predicate (locations-different gra_bay1 gro_bay2)
+set predicate (locations-different gra_bay1 gro_bay3)
+set predicate (locations-different gra_bay1 gro_bay4)
 set predicate (locations-different gra_bay2 gra_bay0)
 set predicate (locations-different gra_bay2 gra_bay1)
 set predicate (locations-different gra_bay2 gra_bay3)
@@ -68,6 +98,11 @@ set predicate (locations-different gra_bay2 gra_bay5)
 set predicate (locations-different gra_bay2 gra_bay6)
 set predicate (locations-different gra_bay2 gra_bay7)
 set predicate (locations-different gra_bay2 gra_bay8)
+set predicate (locations-different gra_bay2 gro_bay0)
+set predicate (locations-different gra_bay2 gro_bay1)
+set predicate (locations-different gra_bay2 gro_bay2)
+set predicate (locations-different gra_bay2 gro_bay3)
+set predicate (locations-different gra_bay2 gro_bay4)
 set predicate (locations-different gra_bay3 gra_bay0)
 set predicate (locations-different gra_bay3 gra_bay1)
 set predicate (locations-different gra_bay3 gra_bay2)
@@ -76,6 +111,11 @@ set predicate (locations-different gra_bay3 gra_bay5)
 set predicate (locations-different gra_bay3 gra_bay6)
 set predicate (locations-different gra_bay3 gra_bay7)
 set predicate (locations-different gra_bay3 gra_bay8)
+set predicate (locations-different gra_bay3 gro_bay0)
+set predicate (locations-different gra_bay3 gro_bay1)
+set predicate (locations-different gra_bay3 gro_bay2)
+set predicate (locations-different gra_bay3 gro_bay3)
+set predicate (locations-different gra_bay3 gro_bay4)
 set predicate (locations-different gra_bay4 gra_bay0)
 set predicate (locations-different gra_bay4 gra_bay1)
 set predicate (locations-different gra_bay4 gra_bay2)
@@ -84,6 +124,11 @@ set predicate (locations-different gra_bay4 gra_bay5)
 set predicate (locations-different gra_bay4 gra_bay6)
 set predicate (locations-different gra_bay4 gra_bay7)
 set predicate (locations-different gra_bay4 gra_bay8)
+set predicate (locations-different gra_bay4 gro_bay0)
+set predicate (locations-different gra_bay4 gro_bay1)
+set predicate (locations-different gra_bay4 gro_bay2)
+set predicate (locations-different gra_bay4 gro_bay3)
+set predicate (locations-different gra_bay4 gro_bay4)
 set predicate (locations-different gra_bay5 gra_bay0)
 set predicate (locations-different gra_bay5 gra_bay1)
 set predicate (locations-different gra_bay5 gra_bay2)
@@ -92,6 +137,11 @@ set predicate (locations-different gra_bay5 gra_bay4)
 set predicate (locations-different gra_bay5 gra_bay6)
 set predicate (locations-different gra_bay5 gra_bay7)
 set predicate (locations-different gra_bay5 gra_bay8)
+set predicate (locations-different gra_bay5 gro_bay0)
+set predicate (locations-different gra_bay5 gro_bay1)
+set predicate (locations-different gra_bay5 gro_bay2)
+set predicate (locations-different gra_bay5 gro_bay3)
+set predicate (locations-different gra_bay5 gro_bay4)
 set predicate (locations-different gra_bay6 gra_bay0)
 set predicate (locations-different gra_bay6 gra_bay1)
 set predicate (locations-different gra_bay6 gra_bay2)
@@ -100,6 +150,11 @@ set predicate (locations-different gra_bay6 gra_bay4)
 set predicate (locations-different gra_bay6 gra_bay5)
 set predicate (locations-different gra_bay6 gra_bay7)
 set predicate (locations-different gra_bay6 gra_bay8)
+set predicate (locations-different gra_bay6 gro_bay0)
+set predicate (locations-different gra_bay6 gro_bay1)
+set predicate (locations-different gra_bay6 gro_bay2)
+set predicate (locations-different gra_bay6 gro_bay3)
+set predicate (locations-different gra_bay6 gro_bay4)
 set predicate (locations-different gra_bay7 gra_bay0)
 set predicate (locations-different gra_bay7 gra_bay1)
 set predicate (locations-different gra_bay7 gra_bay2)
@@ -108,6 +163,11 @@ set predicate (locations-different gra_bay7 gra_bay4)
 set predicate (locations-different gra_bay7 gra_bay5)
 set predicate (locations-different gra_bay7 gra_bay6)
 set predicate (locations-different gra_bay7 gra_bay8)
+set predicate (locations-different gra_bay7 gro_bay0)
+set predicate (locations-different gra_bay7 gro_bay1)
+set predicate (locations-different gra_bay7 gro_bay2)
+set predicate (locations-different gra_bay7 gro_bay3)
+set predicate (locations-different gra_bay7 gro_bay4)
 set predicate (locations-different gra_bay8 gra_bay0)
 set predicate (locations-different gra_bay8 gra_bay1)
 set predicate (locations-different gra_bay8 gra_bay2)
@@ -116,6 +176,76 @@ set predicate (locations-different gra_bay8 gra_bay4)
 set predicate (locations-different gra_bay8 gra_bay5)
 set predicate (locations-different gra_bay8 gra_bay6)
 set predicate (locations-different gra_bay8 gra_bay7)
+set predicate (locations-different gra_bay8 gro_bay0)
+set predicate (locations-different gra_bay8 gro_bay1)
+set predicate (locations-different gra_bay8 gro_bay2)
+set predicate (locations-different gra_bay8 gro_bay3)
+set predicate (locations-different gra_bay8 gro_bay4)
+set predicate (locations-different gro_bay0 gra_bay0)
+set predicate (locations-different gro_bay0 gra_bay1)
+set predicate (locations-different gro_bay0 gra_bay2)
+set predicate (locations-different gro_bay0 gra_bay3)
+set predicate (locations-different gro_bay0 gra_bay4)
+set predicate (locations-different gro_bay0 gra_bay5)
+set predicate (locations-different gro_bay0 gra_bay6)
+set predicate (locations-different gro_bay0 gra_bay7)
+set predicate (locations-different gro_bay0 gra_bay8)
+set predicate (locations-different gro_bay0 gro_bay1)
+set predicate (locations-different gro_bay0 gro_bay2)
+set predicate (locations-different gro_bay0 gro_bay3)
+set predicate (locations-different gro_bay0 gro_bay4)
+set predicate (locations-different gro_bay1 gra_bay0)
+set predicate (locations-different gro_bay1 gra_bay1)
+set predicate (locations-different gro_bay1 gra_bay2)
+set predicate (locations-different gro_bay1 gra_bay3)
+set predicate (locations-different gro_bay1 gra_bay4)
+set predicate (locations-different gro_bay1 gra_bay5)
+set predicate (locations-different gro_bay1 gra_bay6)
+set predicate (locations-different gro_bay1 gra_bay7)
+set predicate (locations-different gro_bay1 gra_bay8)
+set predicate (locations-different gro_bay1 gro_bay0)
+set predicate (locations-different gro_bay1 gro_bay2)
+set predicate (locations-different gro_bay1 gro_bay3)
+set predicate (locations-different gro_bay1 gro_bay4)
+set predicate (locations-different gro_bay2 gra_bay0)
+set predicate (locations-different gro_bay2 gra_bay1)
+set predicate (locations-different gro_bay2 gra_bay2)
+set predicate (locations-different gro_bay2 gra_bay3)
+set predicate (locations-different gro_bay2 gra_bay4)
+set predicate (locations-different gro_bay2 gra_bay5)
+set predicate (locations-different gro_bay2 gra_bay6)
+set predicate (locations-different gro_bay2 gra_bay7)
+set predicate (locations-different gro_bay2 gra_bay8)
+set predicate (locations-different gro_bay2 gro_bay0)
+set predicate (locations-different gro_bay2 gro_bay1)
+set predicate (locations-different gro_bay2 gro_bay3)
+set predicate (locations-different gro_bay2 gro_bay4)
+set predicate (locations-different gro_bay3 gra_bay0)
+set predicate (locations-different gro_bay3 gra_bay1)
+set predicate (locations-different gro_bay3 gra_bay2)
+set predicate (locations-different gro_bay3 gra_bay3)
+set predicate (locations-different gro_bay3 gra_bay4)
+set predicate (locations-different gro_bay3 gra_bay5)
+set predicate (locations-different gro_bay3 gra_bay6)
+set predicate (locations-different gro_bay3 gra_bay7)
+set predicate (locations-different gro_bay3 gra_bay8)
+set predicate (locations-different gro_bay3 gro_bay0)
+set predicate (locations-different gro_bay3 gro_bay1)
+set predicate (locations-different gro_bay3 gro_bay2)
+set predicate (locations-different gro_bay3 gro_bay4)
+set predicate (locations-different gro_bay4 gra_bay0)
+set predicate (locations-different gro_bay4 gra_bay1)
+set predicate (locations-different gro_bay4 gra_bay2)
+set predicate (locations-different gro_bay4 gra_bay3)
+set predicate (locations-different gro_bay4 gra_bay4)
+set predicate (locations-different gro_bay4 gra_bay5)
+set predicate (locations-different gro_bay4 gra_bay6)
+set predicate (locations-different gro_bay4 gra_bay7)
+set predicate (locations-different gro_bay4 gra_bay8)
+set predicate (locations-different gro_bay4 gro_bay0)
+set predicate (locations-different gro_bay4 gro_bay1)
+set predicate (locations-different gro_bay4 gro_bay2)
+set predicate (locations-different gro_bay4 gro_bay3)
 set predicate (robot-available bsharp)
 set predicate (robot-available wannabee)
 set predicate (robot-at bsharp berth1_g)
@@ -129,8 +259,13 @@ set predicate (location-available gra_bay5)
 set predicate (location-available gra_bay6)
 set predicate (location-available gra_bay7)
 set predicate (location-available gra_bay8)
-set predicate (need-stereo bsharp o1 gra_bay1 gra_bay3)
-set predicate (need-stereo wannabee o4 gra_bay5 gra_bay7)
+set predicate (location-available gro_bay0)
+set predicate (location-available gro_bay1)
+set predicate (location-available gro_bay2)
+set predicate (location-available gro_bay3)
+set predicate (location-available gro_bay4)
+set predicate (need-stereo bsharp o1 gro_bay1 gro_bay3)
+set predicate (need-stereo wannabee o4 gra_bay1 gra_bay3)
 set function (= (order-identity o0) 0)
 set function (= (order-identity o1) 1)
 set function (= (order-identity o2) 2)
diff --git a/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl b/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl
new file mode 100644
index 00000000..1611d2a7
--- /dev/null
+++ b/astrobee/survey/survey_manager/pddl/problem_usl_survey.ps2.pddl
@@ -0,0 +1,594 @@
+set instance jem_bay0 location
+set instance jem_bay1 location
+set instance jem_bay2 location
+set instance jem_bay3 location
+set instance jem_bay4 location
+set instance jem_bay5 location
+set instance jem_bay6 location
+set instance jem_bay7 location
+set instance jem_bay8 location
+set instance nod2_bay1 location
+set instance nod2_bay2 location
+set instance nod2_bay3 location
+set instance nod2_bay4 location
+set instance nod2_bay5 location
+set instance usl_bay0 location
+set instance usl_bay1 location
+set instance usl_bay2 location
+set instance usl_bay3 location
+set instance usl_bay4 location
+set instance usl_bay5 location
+set instance usl_bay6 location
+set instance usl_bay7 location
+set instance berth1 location
+set instance berth2 location
+set instance bumble robot
+set instance honey robot
+set instance o0 order
+set instance o1 order
+set instance o2 order
+set instance o3 order
+set instance o4 order
+set goal (and (completed-panorama bumble o1 usl_bay3) (completed-panorama bumble o2 usl_bay2) (completed-panorama bumble o3 usl_bay1) (robot-at bumble berth1) (completed-panorama honey o1 usl_bay6) (completed-panorama honey o2 usl_bay5) (completed-panorama honey o3 usl_bay4) (completed-stereo honey o4 usl_bay1 usl_bay6) (robot-at honey berth2))
+set predicate (move-connected jem_bay0 jem_bay1)
+set predicate (move-connected jem_bay1 jem_bay0)
+set predicate (move-connected jem_bay1 jem_bay2)
+set predicate (move-connected jem_bay2 jem_bay1)
+set predicate (move-connected jem_bay2 jem_bay3)
+set predicate (move-connected jem_bay3 jem_bay2)
+set predicate (move-connected jem_bay3 jem_bay4)
+set predicate (move-connected jem_bay4 jem_bay3)
+set predicate (move-connected jem_bay4 jem_bay5)
+set predicate (move-connected jem_bay5 jem_bay4)
+set predicate (move-connected jem_bay5 jem_bay6)
+set predicate (move-connected jem_bay6 jem_bay5)
+set predicate (move-connected jem_bay6 jem_bay7)
+set predicate (move-connected jem_bay7 jem_bay6)
+set predicate (move-connected jem_bay7 jem_bay8)
+set predicate (move-connected jem_bay8 jem_bay7)
+set predicate (move-connected jem_bay0 nod2_bay1)
+set predicate (move-connected nod2_bay1 jem_bay0)
+set predicate (move-connected nod2_bay1 nod2_bay2)
+set predicate (move-connected nod2_bay2 nod2_bay1)
+set predicate (move-connected nod2_bay2 nod2_bay3)
+set predicate (move-connected nod2_bay3 nod2_bay2)
+set predicate (move-connected nod2_bay3 nod2_bay4)
+set predicate (move-connected nod2_bay4 nod2_bay3)
+set predicate (move-connected nod2_bay4 nod2_bay5)
+set predicate (move-connected nod2_bay5 nod2_bay4)
+set predicate (move-connected nod2_bay5 usl_bay0)
+set predicate (move-connected usl_bay0 nod2_bay5)
+set predicate (move-connected usl_bay0 usl_bay1)
+set predicate (move-connected usl_bay1 usl_bay0)
+set predicate (move-connected usl_bay1 usl_bay2)
+set predicate (move-connected usl_bay2 usl_bay1)
+set predicate (move-connected usl_bay2 usl_bay3)
+set predicate (move-connected usl_bay3 usl_bay2)
+set predicate (move-connected usl_bay3 usl_bay4)
+set predicate (move-connected usl_bay4 usl_bay3)
+set predicate (move-connected usl_bay4 usl_bay5)
+set predicate (move-connected usl_bay5 usl_bay4)
+set predicate (move-connected usl_bay5 usl_bay6)
+set predicate (move-connected usl_bay6 usl_bay5)
+set predicate (move-connected usl_bay6 usl_bay7)
+set predicate (move-connected usl_bay7 usl_bay6)
+set predicate (location-real jem_bay0)
+set predicate (location-real jem_bay1)
+set predicate (location-real jem_bay2)
+set predicate (location-real jem_bay3)
+set predicate (location-real jem_bay4)
+set predicate (location-real jem_bay5)
+set predicate (location-real jem_bay6)
+set predicate (location-real jem_bay7)
+set predicate (location-real nod2_bay1)
+set predicate (location-real nod2_bay2)
+set predicate (location-real nod2_bay3)
+set predicate (location-real nod2_bay4)
+set predicate (location-real nod2_bay5)
+set predicate (location-real usl_bay0)
+set predicate (location-real usl_bay1)
+set predicate (location-real usl_bay2)
+set predicate (location-real usl_bay3)
+set predicate (location-real usl_bay4)
+set predicate (location-real usl_bay5)
+set predicate (location-real usl_bay6)
+set predicate (dock-connected jem_bay7 berth1)
+set predicate (dock-connected jem_bay7 berth2)
+set predicate (robots-different bumble honey)
+set predicate (robots-different honey bumble)
+set predicate (locations-different jem_bay0 jem_bay1)
+set predicate (locations-different jem_bay0 jem_bay2)
+set predicate (locations-different jem_bay0 jem_bay3)
+set predicate (locations-different jem_bay0 jem_bay4)
+set predicate (locations-different jem_bay0 jem_bay5)
+set predicate (locations-different jem_bay0 jem_bay6)
+set predicate (locations-different jem_bay0 jem_bay7)
+set predicate (locations-different jem_bay0 jem_bay8)
+set predicate (locations-different jem_bay0 nod2_bay1)
+set predicate (locations-different jem_bay0 nod2_bay2)
+set predicate (locations-different jem_bay0 nod2_bay3)
+set predicate (locations-different jem_bay0 nod2_bay4)
+set predicate (locations-different jem_bay0 nod2_bay5)
+set predicate (locations-different jem_bay0 usl_bay0)
+set predicate (locations-different jem_bay0 usl_bay1)
+set predicate (locations-different jem_bay0 usl_bay2)
+set predicate (locations-different jem_bay0 usl_bay3)
+set predicate (locations-different jem_bay0 usl_bay4)
+set predicate (locations-different jem_bay0 usl_bay5)
+set predicate (locations-different jem_bay0 usl_bay6)
+set predicate (locations-different jem_bay0 usl_bay7)
+set predicate (locations-different jem_bay1 jem_bay0)
+set predicate (locations-different jem_bay1 jem_bay2)
+set predicate (locations-different jem_bay1 jem_bay3)
+set predicate (locations-different jem_bay1 jem_bay4)
+set predicate (locations-different jem_bay1 jem_bay5)
+set predicate (locations-different jem_bay1 jem_bay6)
+set predicate (locations-different jem_bay1 jem_bay7)
+set predicate (locations-different jem_bay1 jem_bay8)
+set predicate (locations-different jem_bay1 nod2_bay1)
+set predicate (locations-different jem_bay1 nod2_bay2)
+set predicate (locations-different jem_bay1 nod2_bay3)
+set predicate (locations-different jem_bay1 nod2_bay4)
+set predicate (locations-different jem_bay1 nod2_bay5)
+set predicate (locations-different jem_bay1 usl_bay0)
+set predicate (locations-different jem_bay1 usl_bay1)
+set predicate (locations-different jem_bay1 usl_bay2)
+set predicate (locations-different jem_bay1 usl_bay3)
+set predicate (locations-different jem_bay1 usl_bay4)
+set predicate (locations-different jem_bay1 usl_bay5)
+set predicate (locations-different jem_bay1 usl_bay6)
+set predicate (locations-different jem_bay1 usl_bay7)
+set predicate (locations-different jem_bay2 jem_bay0)
+set predicate (locations-different jem_bay2 jem_bay1)
+set predicate (locations-different jem_bay2 jem_bay3)
+set predicate (locations-different jem_bay2 jem_bay4)
+set predicate (locations-different jem_bay2 jem_bay5)
+set predicate (locations-different jem_bay2 jem_bay6)
+set predicate (locations-different jem_bay2 jem_bay7)
+set predicate (locations-different jem_bay2 jem_bay8)
+set predicate (locations-different jem_bay2 nod2_bay1)
+set predicate (locations-different jem_bay2 nod2_bay2)
+set predicate (locations-different jem_bay2 nod2_bay3)
+set predicate (locations-different jem_bay2 nod2_bay4)
+set predicate (locations-different jem_bay2 nod2_bay5)
+set predicate (locations-different jem_bay2 usl_bay0)
+set predicate (locations-different jem_bay2 usl_bay1)
+set predicate (locations-different jem_bay2 usl_bay2)
+set predicate (locations-different jem_bay2 usl_bay3)
+set predicate (locations-different jem_bay2 usl_bay4)
+set predicate (locations-different jem_bay2 usl_bay5)
+set predicate (locations-different jem_bay2 usl_bay6)
+set predicate (locations-different jem_bay2 usl_bay7)
+set predicate (locations-different jem_bay3 jem_bay0)
+set predicate (locations-different jem_bay3 jem_bay1)
+set predicate (locations-different jem_bay3 jem_bay2)
+set predicate (locations-different jem_bay3 jem_bay4)
+set predicate (locations-different jem_bay3 jem_bay5)
+set predicate (locations-different jem_bay3 jem_bay6)
+set predicate (locations-different jem_bay3 jem_bay7)
+set predicate (locations-different jem_bay3 jem_bay8)
+set predicate (locations-different jem_bay3 nod2_bay1)
+set predicate (locations-different jem_bay3 nod2_bay2)
+set predicate (locations-different jem_bay3 nod2_bay3)
+set predicate (locations-different jem_bay3 nod2_bay4)
+set predicate (locations-different jem_bay3 nod2_bay5)
+set predicate (locations-different jem_bay3 usl_bay0)
+set predicate (locations-different jem_bay3 usl_bay1)
+set predicate (locations-different jem_bay3 usl_bay2)
+set predicate (locations-different jem_bay3 usl_bay3)
+set predicate (locations-different jem_bay3 usl_bay4)
+set predicate (locations-different jem_bay3 usl_bay5)
+set predicate (locations-different jem_bay3 usl_bay6)
+set predicate (locations-different jem_bay3 usl_bay7)
+set predicate (locations-different jem_bay4 jem_bay0)
+set predicate (locations-different jem_bay4 jem_bay1)
+set predicate (locations-different jem_bay4 jem_bay2)
+set predicate (locations-different jem_bay4 jem_bay3)
+set predicate (locations-different jem_bay4 jem_bay5)
+set predicate (locations-different jem_bay4 jem_bay6)
+set predicate (locations-different jem_bay4 jem_bay7)
+set predicate (locations-different jem_bay4 jem_bay8)
+set predicate (locations-different jem_bay4 nod2_bay1)
+set predicate (locations-different jem_bay4 nod2_bay2)
+set predicate (locations-different jem_bay4 nod2_bay3)
+set predicate (locations-different jem_bay4 nod2_bay4)
+set predicate (locations-different jem_bay4 nod2_bay5)
+set predicate (locations-different jem_bay4 usl_bay0)
+set predicate (locations-different jem_bay4 usl_bay1)
+set predicate (locations-different jem_bay4 usl_bay2)
+set predicate (locations-different jem_bay4 usl_bay3)
+set predicate (locations-different jem_bay4 usl_bay4)
+set predicate (locations-different jem_bay4 usl_bay5)
+set predicate (locations-different jem_bay4 usl_bay6)
+set predicate (locations-different jem_bay4 usl_bay7)
+set predicate (locations-different jem_bay5 jem_bay0)
+set predicate (locations-different jem_bay5 jem_bay1)
+set predicate (locations-different jem_bay5 jem_bay2)
+set predicate (locations-different jem_bay5 jem_bay3)
+set predicate (locations-different jem_bay5 jem_bay4)
+set predicate (locations-different jem_bay5 jem_bay6)
+set predicate (locations-different jem_bay5 jem_bay7)
+set predicate (locations-different jem_bay5 jem_bay8)
+set predicate (locations-different jem_bay5 nod2_bay1)
+set predicate (locations-different jem_bay5 nod2_bay2)
+set predicate (locations-different jem_bay5 nod2_bay3)
+set predicate (locations-different jem_bay5 nod2_bay4)
+set predicate (locations-different jem_bay5 nod2_bay5)
+set predicate (locations-different jem_bay5 usl_bay0)
+set predicate (locations-different jem_bay5 usl_bay1)
+set predicate (locations-different jem_bay5 usl_bay2)
+set predicate (locations-different jem_bay5 usl_bay3)
+set predicate (locations-different jem_bay5 usl_bay4)
+set predicate (locations-different jem_bay5 usl_bay5)
+set predicate (locations-different jem_bay5 usl_bay6)
+set predicate (locations-different jem_bay5 usl_bay7)
+set predicate (locations-different jem_bay6 jem_bay0)
+set predicate (locations-different jem_bay6 jem_bay1)
+set predicate (locations-different jem_bay6 jem_bay2)
+set predicate (locations-different jem_bay6 jem_bay3)
+set predicate (locations-different jem_bay6 jem_bay4)
+set predicate (locations-different jem_bay6 jem_bay5)
+set predicate (locations-different jem_bay6 jem_bay7)
+set predicate (locations-different jem_bay6 jem_bay8)
+set predicate (locations-different jem_bay6 nod2_bay1)
+set predicate (locations-different jem_bay6 nod2_bay2)
+set predicate (locations-different jem_bay6 nod2_bay3)
+set predicate (locations-different jem_bay6 nod2_bay4)
+set predicate (locations-different jem_bay6 nod2_bay5)
+set predicate (locations-different jem_bay6 usl_bay0)
+set predicate (locations-different jem_bay6 usl_bay1)
+set predicate (locations-different jem_bay6 usl_bay2)
+set predicate (locations-different jem_bay6 usl_bay3)
+set predicate (locations-different jem_bay6 usl_bay4)
+set predicate (locations-different jem_bay6 usl_bay5)
+set predicate (locations-different jem_bay6 usl_bay6)
+set predicate (locations-different jem_bay6 usl_bay7)
+set predicate (locations-different jem_bay7 jem_bay0)
+set predicate (locations-different jem_bay7 jem_bay1)
+set predicate (locations-different jem_bay7 jem_bay2)
+set predicate (locations-different jem_bay7 jem_bay3)
+set predicate (locations-different jem_bay7 jem_bay4)
+set predicate (locations-different jem_bay7 jem_bay5)
+set predicate (locations-different jem_bay7 jem_bay6)
+set predicate (locations-different jem_bay7 jem_bay8)
+set predicate (locations-different jem_bay7 nod2_bay1)
+set predicate (locations-different jem_bay7 nod2_bay2)
+set predicate (locations-different jem_bay7 nod2_bay3)
+set predicate (locations-different jem_bay7 nod2_bay4)
+set predicate (locations-different jem_bay7 nod2_bay5)
+set predicate (locations-different jem_bay7 usl_bay0)
+set predicate (locations-different jem_bay7 usl_bay1)
+set predicate (locations-different jem_bay7 usl_bay2)
+set predicate (locations-different jem_bay7 usl_bay3)
+set predicate (locations-different jem_bay7 usl_bay4)
+set predicate (locations-different jem_bay7 usl_bay5)
+set predicate (locations-different jem_bay7 usl_bay6)
+set predicate (locations-different jem_bay7 usl_bay7)
+set predicate (locations-different jem_bay8 jem_bay0)
+set predicate (locations-different jem_bay8 jem_bay1)
+set predicate (locations-different jem_bay8 jem_bay2)
+set predicate (locations-different jem_bay8 jem_bay3)
+set predicate (locations-different jem_bay8 jem_bay4)
+set predicate (locations-different jem_bay8 jem_bay5)
+set predicate (locations-different jem_bay8 jem_bay6)
+set predicate (locations-different jem_bay8 jem_bay7)
+set predicate (locations-different jem_bay8 nod2_bay1)
+set predicate (locations-different jem_bay8 nod2_bay2)
+set predicate (locations-different jem_bay8 nod2_bay3)
+set predicate (locations-different jem_bay8 nod2_bay4)
+set predicate (locations-different jem_bay8 nod2_bay5)
+set predicate (locations-different jem_bay8 usl_bay0)
+set predicate (locations-different jem_bay8 usl_bay1)
+set predicate (locations-different jem_bay8 usl_bay2)
+set predicate (locations-different jem_bay8 usl_bay3)
+set predicate (locations-different jem_bay8 usl_bay4)
+set predicate (locations-different jem_bay8 usl_bay5)
+set predicate (locations-different jem_bay8 usl_bay6)
+set predicate (locations-different jem_bay8 usl_bay7)
+set predicate (locations-different nod2_bay1 jem_bay0)
+set predicate (locations-different nod2_bay1 jem_bay1)
+set predicate (locations-different nod2_bay1 jem_bay2)
+set predicate (locations-different nod2_bay1 jem_bay3)
+set predicate (locations-different nod2_bay1 jem_bay4)
+set predicate (locations-different nod2_bay1 jem_bay5)
+set predicate (locations-different nod2_bay1 jem_bay6)
+set predicate (locations-different nod2_bay1 jem_bay7)
+set predicate (locations-different nod2_bay1 jem_bay8)
+set predicate (locations-different nod2_bay1 nod2_bay2)
+set predicate (locations-different nod2_bay1 nod2_bay3)
+set predicate (locations-different nod2_bay1 nod2_bay4)
+set predicate (locations-different nod2_bay1 nod2_bay5)
+set predicate (locations-different nod2_bay1 usl_bay0)
+set predicate (locations-different nod2_bay1 usl_bay1)
+set predicate (locations-different nod2_bay1 usl_bay2)
+set predicate (locations-different nod2_bay1 usl_bay3)
+set predicate (locations-different nod2_bay1 usl_bay4)
+set predicate (locations-different nod2_bay1 usl_bay5)
+set predicate (locations-different nod2_bay1 usl_bay6)
+set predicate (locations-different nod2_bay1 usl_bay7)
+set predicate (locations-different nod2_bay2 jem_bay0)
+set predicate (locations-different nod2_bay2 jem_bay1)
+set predicate (locations-different nod2_bay2 jem_bay2)
+set predicate (locations-different nod2_bay2 jem_bay3)
+set predicate (locations-different nod2_bay2 jem_bay4)
+set predicate (locations-different nod2_bay2 jem_bay5)
+set predicate (locations-different nod2_bay2 jem_bay6)
+set predicate (locations-different nod2_bay2 jem_bay7)
+set predicate (locations-different nod2_bay2 jem_bay8)
+set predicate (locations-different nod2_bay2 nod2_bay1)
+set predicate (locations-different nod2_bay2 nod2_bay3)
+set predicate (locations-different nod2_bay2 nod2_bay4)
+set predicate (locations-different nod2_bay2 nod2_bay5)
+set predicate (locations-different nod2_bay2 usl_bay0)
+set predicate (locations-different nod2_bay2 usl_bay1)
+set predicate (locations-different nod2_bay2 usl_bay2)
+set predicate (locations-different nod2_bay2 usl_bay3)
+set predicate (locations-different nod2_bay2 usl_bay4)
+set predicate (locations-different nod2_bay2 usl_bay5)
+set predicate (locations-different nod2_bay2 usl_bay6)
+set predicate (locations-different nod2_bay2 usl_bay7)
+set predicate (locations-different nod2_bay3 jem_bay0)
+set predicate (locations-different nod2_bay3 jem_bay1)
+set predicate (locations-different nod2_bay3 jem_bay2)
+set predicate (locations-different nod2_bay3 jem_bay3)
+set predicate (locations-different nod2_bay3 jem_bay4)
+set predicate (locations-different nod2_bay3 jem_bay5)
+set predicate (locations-different nod2_bay3 jem_bay6)
+set predicate (locations-different nod2_bay3 jem_bay7)
+set predicate (locations-different nod2_bay3 jem_bay8)
+set predicate (locations-different nod2_bay3 nod2_bay1)
+set predicate (locations-different nod2_bay3 nod2_bay2)
+set predicate (locations-different nod2_bay3 nod2_bay4)
+set predicate (locations-different nod2_bay3 nod2_bay5)
+set predicate (locations-different nod2_bay3 usl_bay0)
+set predicate (locations-different nod2_bay3 usl_bay1)
+set predicate (locations-different nod2_bay3 usl_bay2)
+set predicate (locations-different nod2_bay3 usl_bay3)
+set predicate (locations-different nod2_bay3 usl_bay4)
+set predicate (locations-different nod2_bay3 usl_bay5)
+set predicate (locations-different nod2_bay3 usl_bay6)
+set predicate (locations-different nod2_bay3 usl_bay7)
+set predicate (locations-different nod2_bay4 jem_bay0)
+set predicate (locations-different nod2_bay4 jem_bay1)
+set predicate (locations-different nod2_bay4 jem_bay2)
+set predicate (locations-different nod2_bay4 jem_bay3)
+set predicate (locations-different nod2_bay4 jem_bay4)
+set predicate (locations-different nod2_bay4 jem_bay5)
+set predicate (locations-different nod2_bay4 jem_bay6)
+set predicate (locations-different nod2_bay4 jem_bay7)
+set predicate (locations-different nod2_bay4 jem_bay8)
+set predicate (locations-different nod2_bay4 nod2_bay1)
+set predicate (locations-different nod2_bay4 nod2_bay2)
+set predicate (locations-different nod2_bay4 nod2_bay3)
+set predicate (locations-different nod2_bay4 nod2_bay5)
+set predicate (locations-different nod2_bay4 usl_bay0)
+set predicate (locations-different nod2_bay4 usl_bay1)
+set predicate (locations-different nod2_bay4 usl_bay2)
+set predicate (locations-different nod2_bay4 usl_bay3)
+set predicate (locations-different nod2_bay4 usl_bay4)
+set predicate (locations-different nod2_bay4 usl_bay5)
+set predicate (locations-different nod2_bay4 usl_bay6)
+set predicate (locations-different nod2_bay4 usl_bay7)
+set predicate (locations-different nod2_bay5 jem_bay0)
+set predicate (locations-different nod2_bay5 jem_bay1)
+set predicate (locations-different nod2_bay5 jem_bay2)
+set predicate (locations-different nod2_bay5 jem_bay3)
+set predicate (locations-different nod2_bay5 jem_bay4)
+set predicate (locations-different nod2_bay5 jem_bay5)
+set predicate (locations-different nod2_bay5 jem_bay6)
+set predicate (locations-different nod2_bay5 jem_bay7)
+set predicate (locations-different nod2_bay5 jem_bay8)
+set predicate (locations-different nod2_bay5 nod2_bay1)
+set predicate (locations-different nod2_bay5 nod2_bay2)
+set predicate (locations-different nod2_bay5 nod2_bay3)
+set predicate (locations-different nod2_bay5 nod2_bay4)
+set predicate (locations-different nod2_bay5 usl_bay0)
+set predicate (locations-different nod2_bay5 usl_bay1)
+set predicate (locations-different nod2_bay5 usl_bay2)
+set predicate (locations-different nod2_bay5 usl_bay3)
+set predicate (locations-different nod2_bay5 usl_bay4)
+set predicate (locations-different nod2_bay5 usl_bay5)
+set predicate (locations-different nod2_bay5 usl_bay6)
+set predicate (locations-different nod2_bay5 usl_bay7)
+set predicate (locations-different usl_bay0 jem_bay0)
+set predicate (locations-different usl_bay0 jem_bay1)
+set predicate (locations-different usl_bay0 jem_bay2)
+set predicate (locations-different usl_bay0 jem_bay3)
+set predicate (locations-different usl_bay0 jem_bay4)
+set predicate (locations-different usl_bay0 jem_bay5)
+set predicate (locations-different usl_bay0 jem_bay6)
+set predicate (locations-different usl_bay0 jem_bay7)
+set predicate (locations-different usl_bay0 jem_bay8)
+set predicate (locations-different usl_bay0 nod2_bay1)
+set predicate (locations-different usl_bay0 nod2_bay2)
+set predicate (locations-different usl_bay0 nod2_bay3)
+set predicate (locations-different usl_bay0 nod2_bay4)
+set predicate (locations-different usl_bay0 nod2_bay5)
+set predicate (locations-different usl_bay0 usl_bay1)
+set predicate (locations-different usl_bay0 usl_bay2)
+set predicate (locations-different usl_bay0 usl_bay3)
+set predicate (locations-different usl_bay0 usl_bay4)
+set predicate (locations-different usl_bay0 usl_bay5)
+set predicate (locations-different usl_bay0 usl_bay6)
+set predicate (locations-different usl_bay0 usl_bay7)
+set predicate (locations-different usl_bay1 jem_bay0)
+set predicate (locations-different usl_bay1 jem_bay1)
+set predicate (locations-different usl_bay1 jem_bay2)
+set predicate (locations-different usl_bay1 jem_bay3)
+set predicate (locations-different usl_bay1 jem_bay4)
+set predicate (locations-different usl_bay1 jem_bay5)
+set predicate (locations-different usl_bay1 jem_bay6)
+set predicate (locations-different usl_bay1 jem_bay7)
+set predicate (locations-different usl_bay1 jem_bay8)
+set predicate (locations-different usl_bay1 nod2_bay1)
+set predicate (locations-different usl_bay1 nod2_bay2)
+set predicate (locations-different usl_bay1 nod2_bay3)
+set predicate (locations-different usl_bay1 nod2_bay4)
+set predicate (locations-different usl_bay1 nod2_bay5)
+set predicate (locations-different usl_bay1 usl_bay0)
+set predicate (locations-different usl_bay1 usl_bay2)
+set predicate (locations-different usl_bay1 usl_bay3)
+set predicate (locations-different usl_bay1 usl_bay4)
+set predicate (locations-different usl_bay1 usl_bay5)
+set predicate (locations-different usl_bay1 usl_bay6)
+set predicate (locations-different usl_bay1 usl_bay7)
+set predicate (locations-different usl_bay2 jem_bay0)
+set predicate (locations-different usl_bay2 jem_bay1)
+set predicate (locations-different usl_bay2 jem_bay2)
+set predicate (locations-different usl_bay2 jem_bay3)
+set predicate (locations-different usl_bay2 jem_bay4)
+set predicate (locations-different usl_bay2 jem_bay5)
+set predicate (locations-different usl_bay2 jem_bay6)
+set predicate (locations-different usl_bay2 jem_bay7)
+set predicate (locations-different usl_bay2 jem_bay8)
+set predicate (locations-different usl_bay2 nod2_bay1)
+set predicate (locations-different usl_bay2 nod2_bay2)
+set predicate (locations-different usl_bay2 nod2_bay3)
+set predicate (locations-different usl_bay2 nod2_bay4)
+set predicate (locations-different usl_bay2 nod2_bay5)
+set predicate (locations-different usl_bay2 usl_bay0)
+set predicate (locations-different usl_bay2 usl_bay1)
+set predicate (locations-different usl_bay2 usl_bay3)
+set predicate (locations-different usl_bay2 usl_bay4)
+set predicate (locations-different usl_bay2 usl_bay5)
+set predicate (locations-different usl_bay2 usl_bay6)
+set predicate (locations-different usl_bay2 usl_bay7)
+set predicate (locations-different usl_bay3 jem_bay0)
+set predicate (locations-different usl_bay3 jem_bay1)
+set predicate (locations-different usl_bay3 jem_bay2)
+set predicate (locations-different usl_bay3 jem_bay3)
+set predicate (locations-different usl_bay3 jem_bay4)
+set predicate (locations-different usl_bay3 jem_bay5)
+set predicate (locations-different usl_bay3 jem_bay6)
+set predicate (locations-different usl_bay3 jem_bay7)
+set predicate (locations-different usl_bay3 jem_bay8)
+set predicate (locations-different usl_bay3 nod2_bay1)
+set predicate (locations-different usl_bay3 nod2_bay2)
+set predicate (locations-different usl_bay3 nod2_bay3)
+set predicate (locations-different usl_bay3 nod2_bay4)
+set predicate (locations-different usl_bay3 nod2_bay5)
+set predicate (locations-different usl_bay3 usl_bay0)
+set predicate (locations-different usl_bay3 usl_bay1)
+set predicate (locations-different usl_bay3 usl_bay2)
+set predicate (locations-different usl_bay3 usl_bay4)
+set predicate (locations-different usl_bay3 usl_bay5)
+set predicate (locations-different usl_bay3 usl_bay6)
+set predicate (locations-different usl_bay3 usl_bay7)
+set predicate (locations-different usl_bay4 jem_bay0)
+set predicate (locations-different usl_bay4 jem_bay1)
+set predicate (locations-different usl_bay4 jem_bay2)
+set predicate (locations-different usl_bay4 jem_bay3)
+set predicate (locations-different usl_bay4 jem_bay4)
+set predicate (locations-different usl_bay4 jem_bay5)
+set predicate (locations-different usl_bay4 jem_bay6)
+set predicate (locations-different usl_bay4 jem_bay7)
+set predicate (locations-different usl_bay4 jem_bay8)
+set predicate (locations-different usl_bay4 nod2_bay1)
+set predicate (locations-different usl_bay4 nod2_bay2)
+set predicate (locations-different usl_bay4 nod2_bay3)
+set predicate (locations-different usl_bay4 nod2_bay4)
+set predicate (locations-different usl_bay4 nod2_bay5)
+set predicate (locations-different usl_bay4 usl_bay0)
+set predicate (locations-different usl_bay4 usl_bay1)
+set predicate (locations-different usl_bay4 usl_bay2)
+set predicate (locations-different usl_bay4 usl_bay3)
+set predicate (locations-different usl_bay4 usl_bay5)
+set predicate (locations-different usl_bay4 usl_bay6)
+set predicate (locations-different usl_bay4 usl_bay7)
+set predicate (locations-different usl_bay5 jem_bay0)
+set predicate (locations-different usl_bay5 jem_bay1)
+set predicate (locations-different usl_bay5 jem_bay2)
+set predicate (locations-different usl_bay5 jem_bay3)
+set predicate (locations-different usl_bay5 jem_bay4)
+set predicate (locations-different usl_bay5 jem_bay5)
+set predicate (locations-different usl_bay5 jem_bay6)
+set predicate (locations-different usl_bay5 jem_bay7)
+set predicate (locations-different usl_bay5 jem_bay8)
+set predicate (locations-different usl_bay5 nod2_bay1)
+set predicate (locations-different usl_bay5 nod2_bay2)
+set predicate (locations-different usl_bay5 nod2_bay3)
+set predicate (locations-different usl_bay5 nod2_bay4)
+set predicate (locations-different usl_bay5 nod2_bay5)
+set predicate (locations-different usl_bay5 usl_bay0)
+set predicate (locations-different usl_bay5 usl_bay1)
+set predicate (locations-different usl_bay5 usl_bay2)
+set predicate (locations-different usl_bay5 usl_bay3)
+set predicate (locations-different usl_bay5 usl_bay4)
+set predicate (locations-different usl_bay5 usl_bay6)
+set predicate (locations-different usl_bay5 usl_bay7)
+set predicate (locations-different usl_bay6 jem_bay0)
+set predicate (locations-different usl_bay6 jem_bay1)
+set predicate (locations-different usl_bay6 jem_bay2)
+set predicate (locations-different usl_bay6 jem_bay3)
+set predicate (locations-different usl_bay6 jem_bay4)
+set predicate (locations-different usl_bay6 jem_bay5)
+set predicate (locations-different usl_bay6 jem_bay6)
+set predicate (locations-different usl_bay6 jem_bay7)
+set predicate (locations-different usl_bay6 jem_bay8)
+set predicate (locations-different usl_bay6 nod2_bay1)
+set predicate (locations-different usl_bay6 nod2_bay2)
+set predicate (locations-different usl_bay6 nod2_bay3)
+set predicate (locations-different usl_bay6 nod2_bay4)
+set predicate (locations-different usl_bay6 nod2_bay5)
+set predicate (locations-different usl_bay6 usl_bay0)
+set predicate (locations-different usl_bay6 usl_bay1)
+set predicate (locations-different usl_bay6 usl_bay2)
+set predicate (locations-different usl_bay6 usl_bay3)
+set predicate (locations-different usl_bay6 usl_bay4)
+set predicate (locations-different usl_bay6 usl_bay5)
+set predicate (locations-different usl_bay6 usl_bay7)
+set predicate (locations-different usl_bay7 jem_bay0)
+set predicate (locations-different usl_bay7 jem_bay1)
+set predicate (locations-different usl_bay7 jem_bay2)
+set predicate (locations-different usl_bay7 jem_bay3)
+set predicate (locations-different usl_bay7 jem_bay4)
+set predicate (locations-different usl_bay7 jem_bay5)
+set predicate (locations-different usl_bay7 jem_bay6)
+set predicate (locations-different usl_bay7 jem_bay7)
+set predicate (locations-different usl_bay7 jem_bay8)
+set predicate (locations-different usl_bay7 nod2_bay1)
+set predicate (locations-different usl_bay7 nod2_bay2)
+set predicate (locations-different usl_bay7 nod2_bay3)
+set predicate (locations-different usl_bay7 nod2_bay4)
+set predicate (locations-different usl_bay7 nod2_bay5)
+set predicate (locations-different usl_bay7 usl_bay0)
+set predicate (locations-different usl_bay7 usl_bay1)
+set predicate (locations-different usl_bay7 usl_bay2)
+set predicate (locations-different usl_bay7 usl_bay3)
+set predicate (locations-different usl_bay7 usl_bay4)
+set predicate (locations-different usl_bay7 usl_bay5)
+set predicate (locations-different usl_bay7 usl_bay6)
+set predicate (robot-available bumble)
+set predicate (robot-available honey)
+set predicate (robot-at bumble usl_bay1)
+set predicate (robot-at honey usl_bay6)
+set predicate (location-available berth1)
+set predicate (location-available berth2)
+set predicate (location-available jem_bay0)
+set predicate (location-available jem_bay1)
+set predicate (location-available jem_bay2)
+set predicate (location-available jem_bay3)
+set predicate (location-available jem_bay4)
+set predicate (location-available jem_bay5)
+set predicate (location-available jem_bay6)
+set predicate (location-available jem_bay7)
+set predicate (location-available jem_bay8)
+set predicate (location-available nod2_bay1)
+set predicate (location-available nod2_bay2)
+set predicate (location-available nod2_bay3)
+set predicate (location-available nod2_bay4)
+set predicate (location-available nod2_bay5)
+set predicate (location-available usl_bay0)
+set predicate (location-available usl_bay2)
+set predicate (location-available usl_bay3)
+set predicate (location-available usl_bay4)
+set predicate (location-available usl_bay5)
+set predicate (location-available usl_bay7)
+set predicate (need-stereo honey o4 usl_bay1 usl_bay6)
+set function (= (order-identity o0) 0)
+set function (= (order-identity o1) 1)
+set function (= (order-identity o2) 2)
+set function (= (order-identity o3) 3)
+set function (= (order-identity o4) 4)
+set function (= (robot-order bumble) -1)
+set function (= (robot-order honey) -1)
diff --git a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py
index 3bde26cd..e0e3e73f 100755
--- a/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py
+++ b/astrobee/survey/survey_manager/src/survey_manager/command_astrobee.py
@@ -70,45 +70,27 @@ def first_non_zero(a: int, b: int) -> int:
 
 
 def exposure_change(config_static, bay_origin, bay_destination):
-    # Going to JEM
-    if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2":
-        loginfo("CHANGING EXPOSURE TO JEM")
-        return config_static["exposure"]["jem"]
-
-    # Going  to NOD2
-    if (
-        bay_origin == "jem_hatch_to_nod2"
-        and bay_destination == "nod2_hatch_from_jem"
-        or bay_origin == "usl_hatch_to_nod2"
-        and bay_destination == "nod2_hatch_from_usl"
-    ):
-        loginfo("CHANGING EXPOSURE TO NOD2")
-        return config_static["exposure"]["nod2"]
-
-    # Going to USL
-    if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2":
-        return config_static["exposure"]["usl"]
+    if "berth" in bay_origin or "berth" in bay_destination:
+        return 0
+    origin_prefix = bay_origin.split("_")[0]
+    destination_prefix = bay_destination.split("_")[0]
+
+    if origin_prefix != destination_prefix:
+        loginfo(f"CHANGING EXPOSURE TO {destination_prefix.upper()}")
+        return config_static["exposure"][destination_prefix]
 
     return 0
 
 
 def map_change(config_static, bay_origin, bay_destination):
-    # Going to JEM
-    if bay_origin == "nod2_hatch_to_jem" and bay_destination == "jem_hatch_from_nod2":
-        loginfo("CHANGING MAP TO JEM")
-        return config_static["maps"]["jem"]
-    # Going  to NOD2
-    if (
-        bay_origin == "jem_hatch_to_nod2"
-        and bay_destination == "nod2_hatch_from_jem"
-        or bay_origin == "usl_hatch_to_nod2"
-        and bay_destination == "nod2_hatch_from_usl"
-    ):
-        loginfo("CHANGING MAP TO NOD2")
-        return config_static["maps"]["nod2"]
-    # Going to USL
-    if bay_origin == "nod2_hatch_to_usl" and bay_destination == "usl_hatch_from_nod2":
-        return config_static["maps"]["usl"]
+    if "berth" in bay_origin or "berth" in bay_destination:
+        return ""
+    origin_prefix = bay_origin.split("_")[0]
+    destination_prefix = bay_destination.split("_")[0]
+
+    if origin_prefix != destination_prefix:
+        loginfo(f"CHANGING MAP TO {destination_prefix.upper()}")
+        return config_static["maps"][destination_prefix]
     return ""
 
 
@@ -423,10 +405,38 @@ def send_command_recursive(self, command):
 # Mostly used for short actions that should be immediate and require no feedback
 # This method is needed on actions that run remotely and are not controlled by topics
 class CommandExecutor:
-    def __init__(self, ns: str):
-        self.ns = ns
+    def __init__(self, robot: str):
+        self.robot = robot
+        self.ns = None  # Initialize ns, will be set later
+
+    def initialize(self):
+
+        # Figure out robot name and whether we are in simulation or hardware
+        current_robot = os.environ.get("ROBOTNAME")
+        if not current_robot:
+            loginfo(
+                "ROBOTNAME was not defined. Inferring robot from ROS topic /robot_name"
+            )
+            # This is a latching messge so it shouldn't take long
+            try:
+                data = rospy.wait_for_message("/robot_name", String, timeout=5)
+                current_robot = data.data.lower()
+            except:
+                current_robot = ""
+            sim = True
+
+        cmd_exec_ns = ""
+        # If we're commanding a robot remotely
+        if current_robot != self.robot:
+            loginfo(
+                f"We're commanding a namespaced robot! From '{current_robot}' to '{self.robot}'"
+            )
+            # Command executor will add namespace for bridge forwarding
+            cmd_exec_ns = "/" + self.robot
+
+        self.ns = cmd_exec_ns
         loginfo(f"command topic: {self.ns}/command")
-        # Declare guest science command publisher
+
         self.sub_ack = rospy.Subscriber(
             self.ns + "/mgt/ack", AckStamped, self.ack_callback
         )
@@ -442,12 +452,21 @@ def __init__(self, ns: str):
         self.pub_command = rospy.Publisher(
             self.ns + "/command", CommandStamped, queue_size=5
         )
+
+        wait_count = 0
         while self.pub_command.get_num_connections() == 0 and not rospy.is_shutdown():
+            wait_count += 1
+            if wait_count > 10:
+                loginfo(f"Restarting initialization")
+                return False
+
             loginfo(
                 f"Waiting for an astrobee executive to subscribe to {self.ns}/command"
             )
             rospy.sleep(1)
+
         self.unique_cmd_id = ""
+        return True
 
     def start_recording(self, bag_description):
         # Arg is bagfile name description
@@ -484,14 +503,47 @@ def stop_recording(self):
         return result
 
     def change_exposure(self, val):
-        # TBD
+        # Arg is exposure value
+        arg1 = CommandArg()
+        arg1.data_type = CommandArg.DATA_TYPE_STRING
+        arg1.s = CommandConstants.PARAM_NAME_CAMERA_NAME_NAV
+        arg2 = CommandArg()
+        arg2.data_type = CommandArg.DATA_TYPE_FLOAT
+        arg2.f = val
+
+        cmd = CommandStamped()
+        cmd.header = Header(stamp=rospy.Time.now())
+        cmd.cmd_name = CommandConstants.CMD_NAME_SET_EXPOSURE
+        cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec())
+        self.unique_cmd_id = cmd.cmd_id
+        cmd.cmd_src = "isaac fsw"
+        cmd.cmd_origin = "isaac fsw"
+        cmd.args = [arg1, arg2]
+
+        # Publish the CommandStamped message
         loginfo("Change exposure to " + str(val))
-        return 0
+        result = self.publish_and_wait_response(cmd)
+        return result
 
     def change_map(self, map_name):
-        # TBD
+        # Arg is map name
+        arg1 = CommandArg()
+        arg1.data_type = CommandArg.DATA_TYPE_STRING
+        arg1.s = map_name
+
+        cmd = CommandStamped()
+        cmd.header = Header(stamp=rospy.Time.now())
+        cmd.cmd_name = CommandConstants.CMD_NAME_SET_MAP
+        cmd.cmd_id = "survey_manager" + str(rospy.Time.now().to_sec())
+        self.unique_cmd_id = cmd.cmd_id
+        cmd.cmd_src = "isaac fsw"
+        cmd.cmd_origin = "isaac fsw"
+        cmd.args = [arg1]
+
+        # Publish the CommandStamped message
         loginfo("Change map to " + map_name)
-        return 0
+        result = self.publish_and_wait_response(cmd)
+        return result
 
     def ack_callback(self, msg):
         if self.ack_needed is True and msg.cmd_id == self.unique_cmd_id:
@@ -636,32 +688,18 @@ def survey_manager_executor(args, run, config_static, process_executor, quick: b
     rospy.init_node("survey_namager_cmd_" + args["robot"], anonymous=True)
 
     sim = False
-    # Figure out robot name and whether we are in simulation or hardware
-    current_robot = os.environ.get("ROBOTNAME")
-    if not current_robot:
-        loginfo("ROBOTNAME was not defined. Inferring robot from ROS topic /robot_name")
-        # This is a latching messge so it shouldn't take long
-        try:
-            data = rospy.wait_for_message("/robot_name", String, timeout=5)
-            current_robot = data.data.lower()
-        except:
-            current_robot = ""
-        sim = True
-
-    ns = []
-    cmd_exec_ns = ""
-    # If we're commanding a robot remotely
-    if current_robot != args["robot"]:
-        loginfo(
-            f"We're commanding a namespaced robot! From '{current_robot}' to '{args['robot']}'"
-        )
-        ns = ["-ns", args["robot"]]
-        # Command executor will add namespace for bridge forwarding
-        cmd_exec_ns = "/" + args["robot"]
-    command_executor = CommandExecutor(cmd_exec_ns)
+    command_executor = CommandExecutor(args["robot"])
+    if not command_executor.initialize():
+        return -1
+
+    ns = (
+        ["-ns"] + command_executor.ns.lstrip("/").split("/")
+        if command_executor.ns
+        else []
+    )
 
     sm_exec = SurveyManagerExecutor(
-        process_executor, command_executor, config_static, ns, cmd_exec_ns
+        process_executor, command_executor, config_static, ns, command_executor.ns
     )
 
     # Initialize exit code
@@ -874,7 +912,7 @@ class CustomFormatter(argparse.ArgumentDefaultsHelpFormatter):
 def main():
     default_config_paths = [
         os.path.join(
-            rospkg.RosPack().get_path("survey_manager"), "data/jem_survey_static.yaml"
+            rospkg.RosPack().get_path("survey_manager"), "data/iss_survey_static.yaml"
         ),
         os.path.join(
             rospkg.RosPack().get_path("survey_manager"),
diff --git a/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py b/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py
index 6d0e1f20..3703f393 100755
--- a/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py
+++ b/astrobee/survey/survey_manager/src/survey_manager/problem_generator.py
@@ -80,7 +80,7 @@
     os.path.relpath(pathlib.Path(rospack.get_path("survey_manager")) / "pddl", CWD)
 )
 DEFAULT_CONFIGS = [
-    DATA_DIR / "jem_survey_static.yaml",
+    DATA_DIR / "iss_survey_static.yaml",
     DATA_DIR / "jem_survey_dynamic.yaml",
 ]
 
@@ -479,8 +479,8 @@ def problem_generator(
     candidates = (
         ("jem_bay7", "berth1"),
         ("jem_bay7", "berth2"),
-        ("gra_bay3", "berth1"),
-        ("gra_bay5", "berth2"),
+        ("gra_bay4", "berth1_g"),
+        ("gra_bay3", "berth2_g"),
     )
     dock_connected_lines = [
         f"(dock-connected {bay} {berth})"
diff --git a/dense_map/volumetric_mapper/CMakeLists.txt b/dense_map/volumetric_mapper/CMakeLists.txt
index 849905ba..d46555e7 100644
--- a/dense_map/volumetric_mapper/CMakeLists.txt
+++ b/dense_map/volumetric_mapper/CMakeLists.txt
@@ -16,7 +16,7 @@
 # License for the specific language governing permissions and limitations
 # under the License.
 
-cmake_minimum_required(VERSION 2.8.3)
+cmake_minimum_required(VERSION 3.5)
 project(vol_mapper)
 
 ## Compile as C++14, supported in ROS Kinetic and newer
diff --git a/scripts/setup/dependencies/build_install_gp.sh b/scripts/setup/dependencies/build_install_gp.sh
index 3b4fc8a7..cc28688d 100755
--- a/scripts/setup/dependencies/build_install_gp.sh
+++ b/scripts/setup/dependencies/build_install_gp.sh
@@ -23,7 +23,7 @@ PACKAGE_NAME=gp
 if [ -d $PACKAGE_NAME ]; then
   rm -rf $PACKAGE_NAME
 fi
-git clone --quiet https://github.com/marinamrr/libgp.git $PACKAGE_NAME 2>&1 || exit 1
+git clone --quiet https://github.com/marinagmoreira/libgp.git $PACKAGE_NAME 2>&1 || exit 1
 cd $PACKAGE_NAME
 mkdir build && cd build
 cmake ..