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

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

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

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,38 +47,55 @@ 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:
   gra_bay1_to_bay3:
     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

astrobee/survey/survey_manager/data/jem_survey_static.yaml → 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

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

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