Survey manager (#119)

* Plansys2 on Astrobee Noetic (#107) * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Add files via upload Adding planning domain, problem, and xml of the atomic actions of the robotic agents * added files to the correct directory * adding soft constraints problem files * Attempt at a complete MVP domain model * Oops, remove redundant effect * Simplified collision checking predicate * In a working state * Add sample_output_plan.txt * Oops, remove obsolete commented-out code * Fixed stereo survey part of the model based on analysis of old stereo surveys by Marina - unfortunately causes additional planner flakiness * Update sample_output_plan.txt to reflect latest domain/problem * Now generate PDDL problem from higher-level problem specification * Tune panorama estimated duration * Add plan_interpreter.py. Minor cleanup in problem_generator.py * Simplify dynamic config just a bit * Add `survey_manager` subdirectory without layout for survey nodes Includes placeholder directories with hyperlinked `readme.md` files in the documentation hierarchy for the manger, planner, and bridge. Per discussion with @marinagmoreira `survey_manager` lives under `astrobee` to signify the code is intended for cross-compilation. * Improve linking to survey manager docs * Remove `survey_bridge` capability will be added to `astrobee` * Add traclabs plansys2 backport via submodule, thanks @ana-GT * Move sub-modules to `survey_manager` path * Upgrade behaviortree to V4 NOTE: If already installed, remove V3 before installing V4 * Cleanup unused, misplaced sub-modules, again. * Remove `survey_manager` package and organize `survey_planner` * Deprecate Ubuntu-16.04 (xenial) builds of Isaac to support Plansys2 - Remove Ubuntu-16.04 (xenial) CI builds - Update dockerfile ubuntu version defaults to Ubuntu-20.04 (focal) - Update apk build environment to Ubuntu-20.04 (focal) * Fix python formatting * Update to forks of traclabs backports * Add readline development files to setup for plansys2 terminal interface If you have already built your VM, run `sudo apt install libreadline-dev` * Removed outdated PDDL files per @trey0 #107 (comment) * Revert CI upgrades for APK builds --------- Co-authored-by: Abiola Akanni <[email protected]> Co-authored-by: Trey Smith <[email protected]> * Update ci_pr.yml make survey manager PR's run CI * problem_generator.py: Simplified config, and runs with default args again * Removed run number argument from planner actions * Add support for PlanSys2 terminal output; remove num-orders from config * Refactor problem_generator.py to more cleanly support two output formats * plan_interpreter.py works with default arguments again * Update .isort.cfg using scripts/git/configure_isort_paths.sh * jem_survey_dynamic.yaml: Remove unused run parameter. * Fix legacy isort styling error in unrelated file so lint passes * Added let-other-robot-reach constraint so we can solve the full problem with POPF/OPTIC * Fix pylint and mypy complaints * Make predicates on one line. * update submodules * update submodule --------- Co-authored-by: Bckempa <[email protected]> Co-authored-by: Abiola Akanni <[email protected]> Co-authored-by: Trey Smith <[email protected]> Co-authored-by: Brian Coltin <[email protected]>
nasa · Jan 3, 2024 · d6db25d · d6db25d
1 parent bcda63e
commit d6db25d
Show file tree

Hide file tree

Showing 32 changed files with 2,363 additions and 159 deletions.
diff --git a/.github/workflows/apk.yaml b/.github/workflows/apk.yaml
@@ -1,5 +1,3 @@
-# Check for lint error and auto correct them
-
 name: Compile APK
 
 on: ['push', 'pull_request', 'workflow_dispatch']

diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml
@@ -2,32 +2,11 @@ name: Build and Test CI for Pull Requests
 
 on:
   pull_request:
-    branches: [ 'master', 'develop' ]
+    branches: [ 'master', 'develop', 'survey_manager' ]
   workflow_dispatch:
 
 jobs:
 
-  build-xenial:
-
-    runs-on: ubuntu-20.04
-
-    steps:
-
-    - name: Checkout Astrobee
-      uses: actions/checkout@v3
-      with:
-        repository: nasa/astrobee
-        path: astrobee/
-
-    - name: Checkout ISAAC
-      uses: actions/checkout@v3
-      with:
-        submodules: recursive
-        path: isaac/
-
-    - name: Build code for isaac:astrobee Ubuntu 16
-      run: isaac/scripts/docker/build.sh --remote --xenial --astrobee-source-path astrobee/
-
   build-focal:
 
     runs-on: ubuntu-20.04

diff --git a/.github/workflows/ci_push.yml b/.github/workflows/ci_push.yml
@@ -8,65 +8,6 @@ on:
 
 jobs:
 
-  build-xenial:
-
-    runs-on: ubuntu-20.04
-
-    steps:
-
-    - name: Checkout Astrobee
-      uses: actions/checkout@v3
-      with:
-        repository: nasa/astrobee
-        path: astrobee/
-
-    - name: Checkout ISAAC
-      uses: actions/checkout@v3
-      with:
-        submodules: recursive
-        path: isaac/
-
-    - name: Build code for isaac:astrobee Ubuntu 16
-      run: docker build isaac -f isaac/scripts/docker/isaac_astrobee.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=ghcr.io/nasa
-                          -t ghcr.io/${{ github.repository_owner }}/isaac:latest-astrobee-ubuntu16.04
-
-    - name: Build code for isaac:latest Ubuntu 16
-      run: docker build isaac -f isaac/scripts/docker/isaac.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=ghcr.io/nasa
-                          -t ghcr.io/${{ github.repository_owner }}/isaac:latest-ubuntu16.04
-
-    - name: Build messages dockers for Ubuntu 16 (astrobee)
-      run: docker build astrobee -f isaac/scripts/docker/astrobee_msgs.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          -t ghcr.io/${{ github.repository_owner }}/isaac:astrobee-msgs-ubuntu16.04
-
-    - name: Build messages dockers for Ubuntu 16 (isaac)
-      run: docker build isaac -f isaac/scripts/docker/isaac_msgs.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=ghcr.io/nasa
-                          -t ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu16.04
-
-    - name: Log in to registry
-      run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin
-
-    - name: Push Docker image
-      run: |
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:latest-astrobee-ubuntu16.04; fi;
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:latest-ubuntu16.04; fi;
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:astrobee-msgs-ubuntu16.04; fi;
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu16.04; fi;
-
   build-focal:
 
     runs-on: ubuntu-20.04

diff --git a/.github/workflows/ci_release.yml b/.github/workflows/ci_release.yml
@@ -8,68 +8,6 @@ on:
 
 jobs:
 
-  build-xenial:
-
-    runs-on: ubuntu-20.04
-
-    steps:
-    - uses: actions/checkout@v3
-
-    - name: Checkout Astrobee
-      uses: actions/checkout@v3
-      with:
-        repository: nasa/astrobee
-        path: astrobee/
-
-    - name: Checkout ISAAC
-      uses: actions/checkout@v3
-      with:
-        submodules: recursive
-        path: isaac/
-
-    - name: Build code for isaac:astrobee Ubuntu 16
-      run: docker build isaac -f isaac/scripts/docker/isaac_astrobee.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=ghcr.io/nasa
-                          -t isaac/isaac:latest-astrobee-ubuntu16.04
-
-    - name: Build code for isaac:latest Ubuntu 16
-      run: docker build isaac -f isaac/scripts/docker/isaac.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=isaac
-                          -t isaac/isaac:latest-ubuntu16.04
-
-    - name: Build messages dockers for Ubuntu 16 (astrobee)
-      run: docker build astrobee -f isaac/scripts/docker/astrobee_msgs.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          -t isaac/isaac:astrobee-msgs-ubuntu16.04
-
-    - name: Build messages dockers for Ubuntu 16 (isaac)
-      run: docker build isaac -f isaac/scripts/docker/isaac_msgs.Dockerfile
-                          --build-arg UBUNTU_VERSION=16.04
-                          --build-arg ROS_VERSION=kinetic
-                          --build-arg PYTHON=''
-                          --build-arg REMOTE=isaac
-                          -t isaac/isaac:msgs-ubuntu16.04
-
-    - name: Log in to registry
-      run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin
-
-    - name: Push Docker image
-      run: |
-        cd isaac
-        export VERSION=`grep -w -m 1 "Release" RELEASE.md | awk '{print $3}'`
-        docker tag isaac/isaac:latest-astrobee-ubuntu16.04 ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-astrobee-ubuntu16.04
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-astrobee-ubuntu16.04; fi;
-        docker tag isaac/isaac:latest-ubuntu16.04 ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-ubuntu16.04
-        if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:v${VERSION}-ubuntu16.04; fi;
-
   build-focal:
 
     runs-on: ubuntu-20.04

diff --git a/.gitmodules b/.gitmodules
@@ -1,3 +1,11 @@
 [submodule "isaac_msgs"]
 	path = communications/isaac_msgs
 	url = https://github.com/nasa/isaac_msgs.git
+[submodule "astrobee/survey_manager/survey_planner/src/ros2_planning_system"]
+	path = astrobee/survey_manager/survey_planner/src/ros2_planning_system
+	url = https://github.com/bckempa/ros2_planning_system
+	branch = noetic-devel
+[submodule "astrobee/survey_manager/survey_planner/src/ros1_lifecycle"]
+	path = astrobee/survey_manager/survey_planner/src/ros1_lifecycle
+	url = https://github.com/bckempa/ros1_lifecycle
+	branch = noetic-devel
diff --git a/.isort.cfg b/.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
diff --git a/README.md b/README.md
@@ -33,6 +33,7 @@ the [Astrobee robot](https://github.com/nasa/astrobee). This repository includes
 - [Dense mapping](https://nasa.github.io/isaac/html/geometric_streaming_mapper.html) to create a textured 3D map
 - [Volumetric mapping](https://nasa.github.io/isaac/html/volumetric_mapper.html) to map volumetric signals, such as WiFi.
 - [Image analysis](https://nasa.github.io/isaac/html/ano.html) module to train a neural network to detect anomalies
+- [Survey Manager](https://nasa.github.io/isaac/html/survey.html) semi-autonomous planning and execution of imaging tasks.
 
 You may also be interested in the separate repository for the [ISAAC User Interface](https://github.com/nasa/isaac_user_interface),
 which enables monitoring of multiple robots through a web browser.

diff --git a/anomaly/gmm-change-detection/scripts/gmm/gmm_change_detection.py b/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

diff --git a/astrobee/readme.md b/astrobee/readme.md
@@ -9,4 +9,6 @@ The Astrobeemodule contains the ISAAC additions to the Astrobee FSW
 
 \subpage sim : Gazebo plugins for simulation including acoustic and heat camera, RFID reader and emmiter, as well as WiFi reader and emmiters.
 
-\subpage gs_action_helper : Guest science action helper used to communicate with the ground through DDS messages
+\subpage gs_action_helper : Guest science action helper used to communicate with the ground through DDS messages
+
+\subpage survey : Semi-autonomous survey tasking of Astrobees.
diff --git a/astrobee/survey_manager/readme.md b/astrobee/survey_manager/readme.md
@@ -0,0 +1,5 @@
+\page survey Survey Manager
+
+The ISAAC Survey Manager provides semi-autonomous planning and execution of panorama and stereophotography tasks to reduce operator loading and improve activity utilization.
+
+\subpage survey_planner : Planning and scheduling of queued survey actions.
diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml b/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml
@@ -0,0 +1,7 @@
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">
+           <Move    name="collecting_panoramas" goal="${arg2}"/>
+       </Sequence>
+    </BehaviorTree>
+</root>
diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml
@@ -0,0 +1,7 @@
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">
+           <Move    name="move" goal="${arg2}"/>
+       </Sequence>
+    </BehaviorTree>
+</root>
diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml
@@ -0,0 +1,8 @@
+<root BTCPP_format="4" main_tree_to_execute = "MainTree" >
+    <BehaviorTree ID="MainTree">
+       <Sequence name="root_sequence">                                                                 
+           <NavigateNode name="move_to_inspect" robot="${arg0}" pose_id="${arg2}" />
+           <!--<TakePictureNavCam robot="${arg0}" waypoint="${arg2}"/>-->
+       </Sequence>
+    </BehaviorTree>
+</root>
diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml
@@ -0,0 +1,52 @@
+# 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: 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}
+# 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.
+- {type: let_other_robot_reach, robot: honey, order: 0, location: jem_bay5}
+- {type: panorama, robot: honey, order: 1, location: jem_bay7}
+- {type: panorama, robot: honey, order: 2, location: jem_bay6}
+- {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_bay4_to_bay7}
+- {type: robot_at, robot: honey, location: berth2}
+
+init:
+  bumble:
+    location: berth1
+  honey:
+    location: berth2
diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml
@@ -0,0 +1,58 @@
+# 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.
+
+# Static configuration info used when generating a PDDL problem and also when executing actions in a
+# PDDL plan. This info should be static in the sense that it nominally doesn't change during an ISS
+# activity, so the survey manager doesn't have to modify it. However, an edge case is that an
+# operator might want to manually edit something in here (like add a new symbolic location or nudge
+# the position of a named bay away from an obstacle) and restart the survey manager. On the other
+# hand, info that is *expected* to change as part of the survey manager conops belongs in
+# jem_survey_dynamic.yaml.
+
+# Useful reference for positions and stereo survey trajectories:
+# https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure
+
+bays:
+  # 3D coordinates for symbolic bays in ISS Analysis Coordinate System used by Astrobee
+  jem_bay1: [11.0, -4.0, 4.8]
+  jem_bay2: [11.0, -5.0, 4.8]
+  jem_bay3: [11.0, -6.0, 4.8]
+  jem_bay4: [11.0, -7.0, 4.8]
+  jem_bay5: [11.0, -8.0, 4.8]
+  jem_bay6: [11.0, -9.0, 4.8]
+  jem_bay7: [11.0, -9.7, 4.8]
+
+bogus_bays: [jem_bay0, jem_bay8]
+berths: [berth1, berth2]
+robots: [bumble, honey]
+
+stereo:
+  # Meta-data about stereo survey options
+  jem_bay1_to_bay3:
+    # fplan: Name of external fplan specification of trajectory in astrobee_ops/gds/plans/ISAAC/ . The
+    # bay names are intended to indicate which bays are covered by the stereo survey.
+    fplan: "jem_stereo_mapping_bay1_to_bay3.fplan"
+    # base_location: Where trajectory starts and ends for planning purposes (rough location, not exact)
+    base_location: jem_bay1
+    # bound_location: The other end of the interval visited by the trajectory, for planner collision
+    # check purposes.
+    bound_location: jem_bay4  # The survey flies into bay4 even though it only covers up to bay3
+  jem_bay4_to_bay7:
+    fplan: "jem_stereo_mapping_bay4_to_bay7.fplan"
+    base_location: jem_bay7
+    bound_location: jem_bay4