From d6db25ddf58e6e07b7b7fe93b2ba50c4b876ebd2 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Tue, 2 Jan 2024 19:15:31 -0800 Subject: [PATCH] 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 https://github.com/nasa/isaac/pull/107#discussion_r1419159573 * Revert CI upgrades for APK builds --------- Co-authored-by: Abiola Akanni Co-authored-by: Trey Smith * 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 Co-authored-by: Abiola Akanni Co-authored-by: Trey Smith Co-authored-by: Brian Coltin --- .github/workflows/apk.yaml | 2 - .github/workflows/ci_pr.yml | 23 +- .github/workflows/ci_push.yml | 59 --- .github/workflows/ci_release.yml | 62 --- .gitmodules | 8 + .isort.cfg | 2 +- README.md | 1 + .../scripts/gmm/gmm_change_detection.py | 1 + astrobee/readme.md | 4 +- astrobee/survey_manager/readme.md | 5 + .../behavior_trees/collecting_panoramas.xml | 7 + .../survey_planner/behavior_trees/move_to.xml | 7 + .../behavior_trees/move_to_inspect.xml | 8 + .../data/jem_survey_dynamic.yaml | 52 ++ .../data/jem_survey_static.yaml | 58 +++ .../data/sample_output_plan.txt | 154 ++++++ .../data/sample_output_plan.yaml | 339 ++++++++++++ .../survey_planner/pddl/domain_survey.pddl | 353 +++++++++++++ .../pddl/jem_survey_template.pddl | 34 ++ .../pddl/problem_jem_survey.pddl | 282 ++++++++++ .../pddl/problem_jem_survey.ps2.pddl | 139 +++++ .../survey_manager/survey_planner/readme.md | 5 + .../survey_planner/src/ros1_lifecycle | 1 + .../survey_planner/src/ros2_planning_system | 1 + .../survey_planner/tools/plan_interpreter.py | 400 ++++++++++++++ .../survey_planner/tools/problem_generator.py | 487 ++++++++++++++++++ isaac/Subsystems.md | 2 + scripts/docker/astrobee_msgs.Dockerfile | 6 +- scripts/docker/isaac.Dockerfile | 6 +- scripts/docker/isaac_astrobee.Dockerfile | 6 +- scripts/docker/isaac_msgs.Dockerfile | 6 +- scripts/setup/packages_focal.lst | 2 + 32 files changed, 2363 insertions(+), 159 deletions(-) create mode 100644 astrobee/survey_manager/readme.md create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml create mode 100644 astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml create mode 100644 astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml create mode 100644 astrobee/survey_manager/survey_planner/data/jem_survey_static.yaml create mode 100644 astrobee/survey_manager/survey_planner/data/sample_output_plan.txt create mode 100644 astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml create mode 100644 astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl create mode 100644 astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl create mode 100644 astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl create mode 100644 astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl create mode 100644 astrobee/survey_manager/survey_planner/readme.md create mode 160000 astrobee/survey_manager/survey_planner/src/ros1_lifecycle create mode 160000 astrobee/survey_manager/survey_planner/src/ros2_planning_system create mode 100755 astrobee/survey_manager/survey_planner/tools/plan_interpreter.py create mode 100755 astrobee/survey_manager/survey_planner/tools/problem_generator.py diff --git a/.github/workflows/apk.yaml b/.github/workflows/apk.yaml index ea3e5768..9127ee7c 100644 --- 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 index d768e72b..6cc8b9ec 100644 --- 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 index c3fa9ed5..f5a49a6a 100644 --- 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 index c7b48cc9..bcaaa749 100644 --- 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 index b94bfb0e..6d14a890 100644 --- 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 index 46f14eaf..88864386 100644 --- 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 index 3383cae3..b0287805 100644 --- 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 index ab9d6bb7..b0e62348 100755 --- 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 index 89e60ac6..3f2f50d3 100644 --- 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 \ No newline at end of file +\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 new file mode 100644 index 00000000..75b544a4 --- /dev/null +++ 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 new file mode 100644 index 00000000..d8cb181f --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/collecting_panoramas.xml @@ -0,0 +1,7 @@ + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml new file mode 100644 index 00000000..a708b178 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/move_to.xml @@ -0,0 +1,7 @@ + + + + + + + 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 new file mode 100644 index 00000000..b40db393 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/behavior_trees/move_to_inspect.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml b/astrobee/survey_manager/survey_planner/data/jem_survey_dynamic.yaml new file mode 100644 index 00000000..316a3101 --- /dev/null +++ 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 new file mode 100644 index 00000000..fbfd7e02 --- /dev/null +++ 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 diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt new file mode 100644 index 00000000..b25de7b0 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.txt @@ -0,0 +1,154 @@ +$ (ulimit -t 10 && time ./optic_planner pddl/domain_survey.pddl pddl/problem_jem_survey.pddl) +Number of literals: 215 +Constructing lookup tables: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] +Post filtering unreachable actions: [10%] [20%] [30%] [40%] [50%] [60%] [70%] [80%] [90%] [100%] +(robot-order bumble) has finite bounds: [-1.000,4.000] +(robot-order honey) has finite bounds: [-1.000,4.000] +Have identified that smaller values of (robot-order bumble) are preferable +Have identified that smaller values of (robot-order honey) are preferable +Seeing if metric is defined in terms of task vars or a minimal value of makespan +- Yes it is +Recognised a monotonic-change-induced limit on -1.000* +- Must be >= the metric +Looking for achievers for goal index 0, fact (completed-panorama bumble o0 jem_bay4) with fID 83 + (panorama bumble o0 jem_bay4) +For limits: literal goal index 0, fact (completed-panorama bumble o0 jem_bay4), could be achieved by operator (panorama bumble o0 jem_bay4), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 1, fact (completed-panorama bumble o1 jem_bay3) with fID 75 + (panorama bumble o1 jem_bay3) +For limits: literal goal index 1, fact (completed-panorama bumble o1 jem_bay3), could be achieved by operator (panorama bumble o1 jem_bay3), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 2, fact (completed-panorama bumble o2 jem_bay2) with fID 67 + (panorama bumble o2 jem_bay2) +For limits: literal goal index 2, fact (completed-panorama bumble o2 jem_bay2), could be achieved by operator (panorama bumble o2 jem_bay2), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 3, fact (completed-panorama bumble o3 jem_bay1) with fID 59 + (panorama bumble o3 jem_bay1) +For limits: literal goal index 3, fact (completed-panorama bumble o3 jem_bay1), could be achieved by operator (panorama bumble o3 jem_bay1), which has other interesting effects (including one on (robot-available bumble) ) +Looking for achievers for goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4) with fID 123 + (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) (stereo bumble o4 jem_bay1 jem_bay4 jem_bay3 jem_bay5) +For limits: literal goal index 4, fact (completed-stereo bumble o4 jem_bay1 jem_bay4), could be achieved by operator (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) +For limits: literal goal index 5, fact (robot-at bumble berth1), is static or a precondition +Looking for achievers for goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5) with fID 155 + (let-other-robot-reach honey o0 jem_bay5 bumble) + Looking at numeric effects of (let-other-robot-reach honey o0 jem_bay5 bumble): 1 and 0 +For limits: literal goal index 6, fact (completed-let-other-robot-reach honey o0 jem_bay5), could be achieved by operator (let-other-robot-reach honey o0 jem_bay5 bumble), which has a non-trivial numeric effect ((robot-order honey) = 0.000) +Looking for achievers for goal index 7, fact (completed-panorama honey o1 jem_bay7) with fID 116 + (panorama honey o1 jem_bay7) +For limits: literal goal index 7, fact (completed-panorama honey o1 jem_bay7), could be achieved by operator (panorama honey o1 jem_bay7), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 8, fact (completed-panorama honey o2 jem_bay6) with fID 108 + (panorama honey o2 jem_bay6) +For limits: literal goal index 8, fact (completed-panorama honey o2 jem_bay6), could be achieved by operator (panorama honey o2 jem_bay6), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 9, fact (completed-panorama honey o3 jem_bay5) with fID 100 + (panorama honey o3 jem_bay5) +For limits: literal goal index 9, fact (completed-panorama honey o3 jem_bay5), could be achieved by operator (panorama honey o3 jem_bay5), which has other interesting effects (including one on (robot-available honey) ) +Looking for achievers for goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4) with fID 124 + (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) (stereo honey o4 jem_bay7 jem_bay4 jem_bay3 jem_bay5) +For limits: literal goal index 10, fact (completed-stereo honey o4 jem_bay7 jem_bay4), could be achieved by operator (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3), which has other interesting effects (including one on (location-available jem_bay4) ) +For limits: literal goal index 11, fact (robot-at honey berth2), is static or a precondition +Assignment numeric effect ((robot-order bumble) = 0.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 0.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 1.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 1.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 2.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 2.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 3.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 3.000) makes effects on 1 be order-dependent +Assignment numeric effect ((robot-order bumble) = 4.000) makes effects on 0 be order-dependent +Assignment numeric effect ((robot-order honey) = 4.000) makes effects on 1 be order-dependent +27% of the ground temporal actions in this problem are compression-safe +Initial heuristic = 29.000, admissible cost estimate 930.008 +b (28.000 | 630.001) +Resorting to best-first search +Running WA* with W = 5.000, not restarting with goal states +b (28.000 | 630.001)b (28.000 | 70.003)b (27.000 | 870.005)b (26.000 | 870.005)b (25.000 | 880.005)b (24.000 | 880.005)b (23.000 | 1670.007)b (22.000 | 1670.007)b (21.000 | 1680.007)b (20.000 | 1680.007)b (19.000 | 2470.009)b (18.000 | 2470.009)b (17.000 | 2470.009)b (16.000 | 3270.011)b (15.000 | 3270.011)b (14.000 | 3870.012)b (13.000 | 3890.013)b (12.000 | 4470.013)b (12.000 | 3930.015)b (11.000 | 3950.016)b (10.000 | 3970.017)b (9.000 | 3990.018)b (7.000 | 4020.019)b (6.000 | 4650.021)b (4.000 | 4890.024)b (3.000 | 4910.025)b (2.000 | 5510.026)b (1.000 | 5510.026)(G) +; LP calculated the cost + +; Plan found with metric 5540.027 +; Theoretical reachable cost 5540.028 +; States evaluated so far: 504 +; States pruned based on pre-heuristic cost lower bound: 0 +; Time 1.64 +0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] +30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] +50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] +70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] +70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +90.005: (panorama bumble o0 jem_bay4) [780.000] +100.005: (panorama honey o1 jem_bay7) [780.000] +870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.007: (panorama bumble o1 jem_bay3) [780.000] +900.007: (panorama honey o2 jem_bay6) [780.000] +1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1680.008: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +1690.009: (panorama bumble o2 jem_bay2) [780.000] +1700.009: (dock honey jem_bay7 berth2) [30.000] +2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2490.011: (panorama bumble o3 jem_bay1) [780.000] +3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] +3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] +3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +3910.015: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +3930.016: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] +3950.017: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] +3970.018: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] +3990.019: (dock bumble jem_bay7 berth1) [30.000] +4020.020: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +4050.021: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +4070.022: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +4090.023: (panorama honey o3 jem_bay5) [780.000] +4870.024: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +4890.025: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +4910.026: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] +5510.027: (dock honey jem_bay7 berth2) [30.000] + + * All goal deadlines now no later than 5540.027 +b (1.000 | 5470.024)(G) +; LP calculated the cost + +; Plan found with metric 5500.025 +; Theoretical reachable cost 5500.026 +; States evaluated so far: 866 +; States pruned based on pre-heuristic cost lower bound: 1 +; Time 2.85 +0.000: (undock bumble berth1 jem_bay7 jem_bay8 jem_bay6) [30.000] +30.001: (move bumble jem_bay7 jem_bay6 jem_bay5) [20.000] +50.002: (move bumble jem_bay6 jem_bay5 jem_bay4) [20.000] +70.003: (let-other-robot-reach honey o0 jem_bay5 bumble) [0.000] +70.004: (move bumble jem_bay5 jem_bay4 jem_bay3) [20.000] +70.004: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +90.005: (panorama bumble o0 jem_bay4) [780.000] +100.005: (panorama honey o1 jem_bay7) [780.000] +870.006: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +880.006: (move honey jem_bay7 jem_bay6 jem_bay5) [20.000] +890.007: (panorama bumble o1 jem_bay3) [780.000] +900.007: (panorama honey o2 jem_bay6) [780.000] +1670.008: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +1690.009: (panorama bumble o2 jem_bay2) [780.000] +2470.010: (move bumble jem_bay2 jem_bay1 jem_bay0) [20.000] +2490.011: (panorama bumble o3 jem_bay1) [780.000] +3270.012: (stereo bumble o4 jem_bay1 jem_bay4 jem_bay5 jem_bay3) [600.000] +3870.013: (move bumble jem_bay1 jem_bay2 jem_bay3) [20.000] +3870.013: (move honey jem_bay6 jem_bay5 jem_bay4) [20.000] +3890.014: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +3890.014: (panorama honey o3 jem_bay5) [780.000] +4670.015: (move honey jem_bay5 jem_bay6 jem_bay7) [20.000] +4690.016: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +4690.016: (move honey jem_bay6 jem_bay7 jem_bay8) [20.000] +4710.017: (dock honey jem_bay7 berth2) [30.000] +4710.017: (move bumble jem_bay4 jem_bay3 jem_bay2) [20.000] +4730.018: (move bumble jem_bay3 jem_bay2 jem_bay1) [20.000] +4740.018: (undock honey berth2 jem_bay7 jem_bay8 jem_bay6) [30.000] +4770.019: (stereo honey o4 jem_bay7 jem_bay4 jem_bay5 jem_bay3) [600.000] +5370.020: (dock honey jem_bay7 berth2) [30.000] +5370.020: (move bumble jem_bay2 jem_bay3 jem_bay4) [20.000] +5390.021: (move bumble jem_bay3 jem_bay4 jem_bay5) [20.000] +5410.022: (move bumble jem_bay4 jem_bay5 jem_bay6) [20.000] +5430.023: (move bumble jem_bay5 jem_bay6 jem_bay7) [20.000] +5450.024: (move bumble jem_bay6 jem_bay7 jem_bay8) [20.000] +5470.025: (dock bumble jem_bay7 berth1) [30.000] + + * All goal deadlines now no later than 5500.025 + +real 0m10.009s +user 0m9.767s +sys 0m0.240s diff --git a/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml new file mode 100644 index 00000000..7c624cb1 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/data/sample_output_plan.yaml @@ -0,0 +1,339 @@ +- start_time_seconds: '0.000' + action: + type: undock + robot: bumble + duration_seconds: '30.000' +- start_time_seconds: '30.001' + action: + type: move + robot: bumble + from_name: jem_bay7 + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '50.002' + action: + type: move + robot: bumble + from_name: jem_bay6 + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '70.004' + action: + type: move + robot: bumble + from_name: jem_bay5 + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '70.004' + action: + type: undock + robot: honey + duration_seconds: '30.000' +- start_time_seconds: '90.005' + action: + type: panorama + robot: bumble + location_name: jem_bay4 + location_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '100.005' + action: + type: panorama + robot: honey + location_name: jem_bay7 + location_pos: + - 11.0 + - -9.7 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '870.006' + action: + type: move + robot: bumble + from_name: jem_bay4 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '880.006' + action: + type: move + robot: honey + from_name: jem_bay7 + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '890.007' + action: + type: panorama + robot: bumble + location_name: jem_bay3 + location_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '900.007' + action: + type: panorama + robot: honey + location_name: jem_bay6 + location_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '1670.008' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '1690.009' + action: + type: panorama + robot: bumble + location_name: jem_bay2 + location_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '2470.010' + action: + type: move + robot: bumble + from_name: jem_bay2 + to_name: jem_bay1 + to_pos: + - 11.0 + - -4.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '2490.011' + action: + type: panorama + robot: bumble + location_name: jem_bay1 + location_pos: + - 11.0 + - -4.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '3270.012' + action: + type: stereo + robot: bumble + fplan: jem_stereo_mapping_bay1_to_bay3.fplan + base_name: jem_bay1 + bound_name: jem_bay4 + duration_seconds: '600.000' +- start_time_seconds: '3870.013' + action: + type: move + robot: bumble + from_name: jem_bay1 + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '3870.013' + action: + type: move + robot: honey + from_name: jem_bay6 + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '3890.014' + action: + type: move + robot: bumble + from_name: jem_bay2 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '3890.014' + action: + type: panorama + robot: honey + location_name: jem_bay5 + location_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '780.000' +- start_time_seconds: '4670.015' + action: + type: move + robot: honey + from_name: jem_bay5 + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4690.016' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4690.016' + action: + type: move + robot: honey + from_name: jem_bay6 + to_name: jem_bay7 + to_pos: + - 11.0 + - -9.7 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4710.017' + action: + type: dock + robot: honey + berth: berth2 + duration_seconds: '30.000' +- start_time_seconds: '4710.017' + action: + type: move + robot: bumble + from_name: jem_bay4 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4730.018' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay2 + to_pos: + - 11.0 + - -5.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '4740.018' + action: + type: undock + robot: honey + duration_seconds: '30.000' +- start_time_seconds: '4770.019' + action: + type: stereo + robot: honey + fplan: jem_stereo_mapping_bay4_to_bay7.fplan + base_name: jem_bay7 + bound_name: jem_bay4 + duration_seconds: '600.000' +- start_time_seconds: '5370.020' + action: + type: dock + robot: honey + berth: berth2 + duration_seconds: '30.000' +- start_time_seconds: '5370.020' + action: + type: move + robot: bumble + from_name: jem_bay2 + to_name: jem_bay3 + to_pos: + - 11.0 + - -6.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5390.021' + action: + type: move + robot: bumble + from_name: jem_bay3 + to_name: jem_bay4 + to_pos: + - 11.0 + - -7.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5410.022' + action: + type: move + robot: bumble + from_name: jem_bay4 + to_name: jem_bay5 + to_pos: + - 11.0 + - -8.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5430.023' + action: + type: move + robot: bumble + from_name: jem_bay5 + to_name: jem_bay6 + to_pos: + - 11.0 + - -9.0 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5450.024' + action: + type: move + robot: bumble + from_name: jem_bay6 + to_name: jem_bay7 + to_pos: + - 11.0 + - -9.7 + - 4.8 + duration_seconds: '20.000' +- start_time_seconds: '5470.025' + action: + type: dock + robot: bumble + berth: berth1 + duration_seconds: '30.000' diff --git a/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl new file mode 100644 index 00000000..5e197671 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/domain_survey.pddl @@ -0,0 +1,353 @@ +(define (domain survey-manager) + (:requirements + :strips + :typing + :durative-actions + :fluents + ) + + (:types + location + robot + order + ) + + (:predicates + ;; === Static predicates === + ;; move-connected: Indicates a robot can travel from ?from to ?to using a single + ;; move action. Note that these are interpreted as directed links, so for the usual case + ;; that you can travel in either direction, you must assert the predicate both ways. + ;; Neither location can be a berth (use the dock-connected predicate for that). However, + ;; both bays 6 and 7 should be move-connected to both berth approach points. + (move-connected ?from ?to - location) + + ;; location-real: Indicates a location is a real place the robot can fly to. We've added + ;; bogus locations bay0 and bay8 to the problem instance to satisfy the implicit assumption + ;; of the collision checking that every bay has two neighbors. We assert location-real for + ;; the other locations and use it as a precondition on moves so the planner can't + ;; accidentally fly to a bogus location. + (location-real ?location - location) + + ;; dock-connected: Indicates a robot can move from ?approach to ?berth using a dock + ;; action, or from ?berth to ?approach using an undock action. + (dock-connected ?approach ?berth - location) + + ;; robots-different: Indicates a != b. Needs to be expressed as a positive predicate + ;; so it can be used as a precondition. Must be asserted in both directions. + (robots-different ?a ?b - robot) + + ;; locations-different: Indicates a != b. Needs to be expressed as a positive predicate + ;; so it can be used as a precondition. Must be asserted in both directions. + (locations-different ?a ?b - location) + + ;; === Dynamic predicates === + ;; robot-available: Since a robot can only perform one action at a time in our domain, each + ;; action grabs this mutex. In the initial state, both robots should be available. + (robot-available ?robot - robot) + + ;; robot-at: Indicates the robot's current position (usually not set during execution of + ;; motion actions). In the initial state, both robots should have robot-at set for their + ;; initial locations. + (robot-at ?robot - robot ?location - location) + + ;; location-available: Indicates that no robot has reserved the location. When stationary, + ;; each robot reserves its current location. During a move, it reserves both its ?from + ;; location and its ?to location. Collision avoidance checks prevent robots from reserving + ;; the same location or reserving adjacent bays while flying. Note: It might be more natural + ;; to express this as a location-reserved predicate with the opposite boolean sense, but we + ;; need it to be this way so we can use it as a precondition without negating it. In the + ;; initial state, we must mark location-available for all locations, real or bogus, except + ;; the initial robot locations. + (location-available ?location - location) + + ;; need-stereo: If you add a completed-stereo goal, you must also add a need-stereo + ;; predicate with identical parameters to the initial state. This is part of a hack that + ;; greatly improves planner performance. The need-stereo predicate has been made part of the + ;; preconditions of the stereo action, and one of its effects is to clear the + ;; predicate. Therefore, the planner won't waste time trying to execute stereo actions that + ;; the user didn't explicitly request. Without this hack, the planner run time blows up. + (need-stereo ?robot - robot ?order - order ?base ?bound - location) + + ;; === Goal predicates === + ;; completed-panorama: The goal to add if you want the plan to include collecting a + ;; panorama. For now, goals specify ?robot and ?order parameters that constrain + ;; multi-robot task allocation and task ordering. + (completed-panorama ?robot - robot ?order - order ?location - location ) + + ;; completed-stereo: The goal to add if you want the plan to include collecting a stereo + ;; survey. For now, goals specify ?robot and ?order parameters that constrain multi-robot + ;; task allocation and task ordering. The current model for stereo surveys assumes the robot + ;; starts and ends the survey at the same location called ?base (these locations only need + ;; to be same to the effective precision modeled in the planner, "less than a bay apart"). + ;; The ?bound argument indicates the other end of the interval covered by the survey and is + ;; used for collision checking. It's assumed that ?base and ?bound are not adjacent + ;; locations. If future stereo surveys violate these assumptions the model will need to be + ;; revisited. + (completed-stereo ?robot - robot ?order - order ?base ?bound - location ) + + ;; completed-let-other-robot-reach: The goal to add if you want one robot to wait for the + ;; other to reach a certain location before pursuing its remaining goals (ones with larger + ;; ?order values). This basically enables a user to provide a specific kind of + ;; between-robots ordering hint to the planner. + (completed-let-other-robot-reach ?robot - robot ?order - order ?loc - location ) + ) + + (:functions + ;; === Static numeric fluents === + ;; order-identity: An identity operator that maps from a symbolic order like o0 to its + ;; corresponding numeric value 0. + (order-identity ?order - order) + + ;; === Dynamic numeric fluents === + ;; robot-order: Indicates the order of the last action executed by ?robot. Later actions + ;; must not have a lower ?order (this only applies to the panorama and stereo actions that + ;; take an ?order parameter). In the initial state, each robot must have order -1. + (robot-order ?robot - robot) + ) + + (:durative-action dock + :parameters (?robot - robot ?from ?to - location) ;; from bay7 to berth1 or berth2 + :duration (= ?duration 30) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (dock-connected ?from ?to)) + + ;; Check collision avoidance + (at start (location-available ?to)) + ; Don't need to check berth neighbors + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action undock + :parameters ( + ?robot - robot + ?from ?to - location ;; from berth1 or berth2 to bay7 + ?check1 ?check2 - location ;; neighbors of ?to to check for collision avoidance + ) + :duration (= ?duration 30) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (dock-connected ?to ?from)) + (at start (location-real ?to)) + + ;(at start (robot-can-undock-now ?robot)) + + ;; Check collision avoidance + (at start (location-available ?to)) + (at start (locations-different ?check1 ?check2)) + (at start (move-connected ?check1 ?to)) + (at start (move-connected ?check2 ?to)) + (at start (location-available ?check1)) + (at start (location-available ?check2)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action move + :parameters ( + ?robot - robot + ?from ?to - location + ?check - location ;; neighbor of ?to to check for collision avoidance + ) + :duration (= ?duration 20) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check parameters make sense + (at start (robot-at ?robot ?from)) + (at start (move-connected ?from ?to)) + (at start (location-real ?to)) + + ;; Check collision avoidance + (at start (location-available ?to)) + ;; In general when flying to ?to we collision check whether ?to or either of its + ;; neighbors are reserved (by the other robot). In this case, one of the neighbors + ;; of ?to is ?from, which can't be reserved by the other robot since this robot + ;; previously reserved it. Therefore, we only need to check if the other neighbor + ;; (?check) is reserved. + (at start (locations-different ?check ?from)) + (at start (move-connected ?check ?to)) + (at start (location-available ?check)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Grab and release reserved locations + (at start (not (location-available ?to))) + (at end (location-available ?from)) + + ;; Update robot location + (at start (not (robot-at ?robot ?from))) + (at end (robot-at ?robot ?to)) + ) + ) + + (:durative-action panorama + :parameters + ( + ?robot - robot + ?order - order + ?location - location + ) + ;; ~13 minutes, per https://babelfish.arc.nasa.gov/confluence/display/FFOPS/ISAAC+Phase+1X+Activity+9+Ground+Procedure + :duration (= ?duration 780) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robot-at ?robot ?location)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ;; Mark success + (at end (completed-panorama ?robot ?order ?location)) + ) + ) + + (:durative-action stereo + :parameters + ( + ?robot - robot + ?order - order + ;; ?base: The bay where we start and also stop. ?bound: The other end of the survey + ?base ?bound - location + ;; ?check1 and ?check2: Planner-selected neighbors of ?bound for collision check + ?check1 ?check2 - location + ) + :duration (= ?duration 600) ;; 10 minutes + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robot-at ?robot ?base)) + (at start (location-real ?bound)) + + ;; Check for need-stereo so the planner only tries this action when the user + ;; explicitly requests it. + (at start (need-stereo ?robot ?order ?base ?bound)) + + ;; Check collision avoidance + (at start (location-available ?bound)) + (at start (locations-different ?check1 ?check2)) + (at start (move-connected ?check1 ?bound)) + (at start (move-connected ?check2 ?bound)) + (at start (location-available ?check1)) + (at start (location-available ?check2)) + ) + :effect + (and + ;; Grab and release robot mutex + (at start (not (robot-available ?robot))) + (at end (robot-available ?robot)) + + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ;; Grab and release reserved locations + (at start (not (location-available ?bound))) + (at end (location-available ?bound)) + + ;; Update robot location - technically correct but not really needed + ;(at start (not (robot-at ?robot ?base))) + ;(at end (robot-at ?robot ?base)) + + ;; Clear need-stereo so the planner won't try to use the stereo action + ;; again after the user request is satisfied. + (at end (not (need-stereo ?robot ?order ?base ?bound))) + + ;; Mark success + (at end (completed-stereo ?robot ?order ?base ?bound)) + ) + ) + + (:durative-action let-other-robot-reach + :parameters ( + ?robot - robot + ?order - order + ?other-loc - location ;; location other robot needs to reach + ?other-robot - robot + ) + :duration (= ?duration 0) + :condition + (and + ;; Check robot mutex + (at start (robot-available ?robot)) + + ;; Check order + (at start (< (robot-order ?robot) (order-identity ?order))) + + ;; Check parameters make sense + (at start (robots-different ?robot ?other-robot)) + + ;; The main point is to wait until this condition is met + (at start (robot-at ?other-robot ?other-loc)) + ) + :effect + (and + ;; Update order + (at end (assign (robot-order ?robot) (order-identity ?order))) + + ; Mark success + (at end (completed-let-other-robot-reach ?robot ?order ?other-loc)) + ) + ) +) diff --git a/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl new file mode 100644 index 00000000..8cc991f1 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/jem_survey_template.pddl @@ -0,0 +1,34 @@ +{{ header }} +(define (problem jem-survey) + (:domain survey-manager) + (:metric minimize (total-time)) + (:objects + {{ objects }} + ) + + (:goal + (and + {{ goals }} + ) + ) + + (:init + ;; === Static predicates === + {{ static_predicates }} + + ;; === Dynamic predicates === + {{ dynamic_predicates }} + + ;; === Static numeric fluents === + {{ static_fluents }} + + ;; === Dynamic numeric fluents === + {{ dynamic_fluents }} + ) ;; end :init +) ;; end problem + +;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. + +;; BEGIN CONFIG +{{ config }} +;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl new file mode 100644 index 00000000..d099ca79 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.pddl @@ -0,0 +1,282 @@ +;; Auto-generated by problem_generator.py. Do not edit! +;; Command was: ./tools/problem_generator.py +;; Working directory was: /home/vagrant/isaac/astrobee/survey_manager/survey_planner +;; Problem template: pddl/jem_survey_template.pddl +;; Config 1: data/jem_survey_static.yaml +;; Config 2: data/jem_survey_dynamic.yaml + +(define (problem jem-survey) + (:domain survey-manager) + (:metric minimize (total-time)) + (:objects + jem_bay0 jem_bay1 jem_bay2 jem_bay3 jem_bay4 jem_bay5 jem_bay6 jem_bay7 jem_bay8 berth1 berth2 - location + bumble honey - robot + o0 o1 o2 o3 o4 - order + ) + + (:goal + (and + (completed-panorama bumble o0 jem_bay4) + (completed-panorama bumble o1 jem_bay3) + (completed-panorama bumble o2 jem_bay2) + (completed-panorama bumble o3 jem_bay1) + (completed-stereo bumble o4 jem_bay1 jem_bay4) + (robot-at bumble berth1) + (completed-let-other-robot-reach honey o0 jem_bay5) + (completed-panorama honey o1 jem_bay7) + (completed-panorama honey o2 jem_bay6) + (completed-panorama honey o3 jem_bay5) + (completed-stereo honey o4 jem_bay7 jem_bay4) + (robot-at honey berth2) + ) + ) + + (:init + ;; === Static predicates === + (move-connected jem_bay0 jem_bay1) + (move-connected jem_bay1 jem_bay0) + (move-connected jem_bay1 jem_bay2) + (move-connected jem_bay2 jem_bay1) + (move-connected jem_bay2 jem_bay3) + (move-connected jem_bay3 jem_bay2) + (move-connected jem_bay3 jem_bay4) + (move-connected jem_bay4 jem_bay3) + (move-connected jem_bay4 jem_bay5) + (move-connected jem_bay5 jem_bay4) + (move-connected jem_bay5 jem_bay6) + (move-connected jem_bay6 jem_bay5) + (move-connected jem_bay6 jem_bay7) + (move-connected jem_bay7 jem_bay6) + (move-connected jem_bay7 jem_bay8) + (move-connected jem_bay8 jem_bay7) + (location-real jem_bay1) + (location-real jem_bay2) + (location-real jem_bay3) + (location-real jem_bay4) + (location-real jem_bay5) + (location-real jem_bay6) + (location-real jem_bay7) + (dock-connected jem_bay7 berth1) + (dock-connected jem_bay7 berth2) + (robots-different bumble honey) + (robots-different honey bumble) + (locations-different jem_bay0 jem_bay1) + (locations-different jem_bay0 jem_bay2) + (locations-different jem_bay0 jem_bay3) + (locations-different jem_bay0 jem_bay4) + (locations-different jem_bay0 jem_bay5) + (locations-different jem_bay0 jem_bay6) + (locations-different jem_bay0 jem_bay7) + (locations-different jem_bay0 jem_bay8) + (locations-different jem_bay1 jem_bay0) + (locations-different jem_bay1 jem_bay2) + (locations-different jem_bay1 jem_bay3) + (locations-different jem_bay1 jem_bay4) + (locations-different jem_bay1 jem_bay5) + (locations-different jem_bay1 jem_bay6) + (locations-different jem_bay1 jem_bay7) + (locations-different jem_bay1 jem_bay8) + (locations-different jem_bay2 jem_bay0) + (locations-different jem_bay2 jem_bay1) + (locations-different jem_bay2 jem_bay3) + (locations-different jem_bay2 jem_bay4) + (locations-different jem_bay2 jem_bay5) + (locations-different jem_bay2 jem_bay6) + (locations-different jem_bay2 jem_bay7) + (locations-different jem_bay2 jem_bay8) + (locations-different jem_bay3 jem_bay0) + (locations-different jem_bay3 jem_bay1) + (locations-different jem_bay3 jem_bay2) + (locations-different jem_bay3 jem_bay4) + (locations-different jem_bay3 jem_bay5) + (locations-different jem_bay3 jem_bay6) + (locations-different jem_bay3 jem_bay7) + (locations-different jem_bay3 jem_bay8) + (locations-different jem_bay4 jem_bay0) + (locations-different jem_bay4 jem_bay1) + (locations-different jem_bay4 jem_bay2) + (locations-different jem_bay4 jem_bay3) + (locations-different jem_bay4 jem_bay5) + (locations-different jem_bay4 jem_bay6) + (locations-different jem_bay4 jem_bay7) + (locations-different jem_bay4 jem_bay8) + (locations-different jem_bay5 jem_bay0) + (locations-different jem_bay5 jem_bay1) + (locations-different jem_bay5 jem_bay2) + (locations-different jem_bay5 jem_bay3) + (locations-different jem_bay5 jem_bay4) + (locations-different jem_bay5 jem_bay6) + (locations-different jem_bay5 jem_bay7) + (locations-different jem_bay5 jem_bay8) + (locations-different jem_bay6 jem_bay0) + (locations-different jem_bay6 jem_bay1) + (locations-different jem_bay6 jem_bay2) + (locations-different jem_bay6 jem_bay3) + (locations-different jem_bay6 jem_bay4) + (locations-different jem_bay6 jem_bay5) + (locations-different jem_bay6 jem_bay7) + (locations-different jem_bay6 jem_bay8) + (locations-different jem_bay7 jem_bay0) + (locations-different jem_bay7 jem_bay1) + (locations-different jem_bay7 jem_bay2) + (locations-different jem_bay7 jem_bay3) + (locations-different jem_bay7 jem_bay4) + (locations-different jem_bay7 jem_bay5) + (locations-different jem_bay7 jem_bay6) + (locations-different jem_bay7 jem_bay8) + (locations-different jem_bay8 jem_bay0) + (locations-different jem_bay8 jem_bay1) + (locations-different jem_bay8 jem_bay2) + (locations-different jem_bay8 jem_bay3) + (locations-different jem_bay8 jem_bay4) + (locations-different jem_bay8 jem_bay5) + (locations-different jem_bay8 jem_bay6) + (locations-different jem_bay8 jem_bay7) + + ;; === Dynamic predicates === + (robot-available bumble) + (robot-available honey) + (robot-at bumble berth1) + (robot-at honey berth2) + (location-available jem_bay0) + (location-available jem_bay1) + (location-available jem_bay2) + (location-available jem_bay3) + (location-available jem_bay4) + (location-available jem_bay5) + (location-available jem_bay6) + (location-available jem_bay7) + (location-available jem_bay8) + (need-stereo bumble o4 jem_bay1 jem_bay4) + (need-stereo honey o4 jem_bay7 jem_bay4) + + ;; === Static numeric fluents === + (= (order-identity o0) 0) + (= (order-identity o1) 1) + (= (order-identity o2) 2) + (= (order-identity o3) 3) + (= (order-identity o4) 4) + + ;; === Dynamic numeric fluents === + (= (robot-order bumble) -1) + (= (robot-order honey) -1) + ) ;; end :init +) ;; end problem + +;; Include raw high-level config in problem in case an (ISAAC-custom) planner prefers to use it. + +;; BEGIN CONFIG +;; # 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/ +;; 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 covered by the trajectory, for planner collision +;; # check purposes. (Note a trajectory may fly a bit into a bay that it doesn't claim to cover. +;; # The two surveys that cover the module purposefully overlap.) +;; bound_location: jem_bay4 +;; jem_bay4_to_bay7: +;; fplan: "jem_stereo_mapping_bay4_to_bay7.fplan" +;; base_location: jem_bay7 +;; bound_location: jem_bay4 +;; # 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} +;; +;; # We want Bumble to return to its berth at the end of the run, but adding this goal causes POPF to +;; # get confused and greatly increase the total run time. For some reason, it doesn't notice it can +;; # use the same plan as without this goal and then add some motion actions at the end to achieve this +;; # goal. Instead, it falls back to only undocking one robot at a time, which slows things down by +;; # about 2x. +;; - {type: robot_at, robot: bumble, location: berth1} +;; +;; - {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 another objective we want to include that for some reason causes POPF to fail to generate +;; # a plan (hang indefinitely). No obvious reason why it should cause a problem. +;; - {type: stereo, robot: honey, order: 4, trajectory: jem_bay4_to_bay7} +;; +;; - {type: robot_at, robot: honey, location: berth2} +;; +;; init: +;; bumble: +;; location: berth1 +;; honey: +;; location: berth2 +;; END CONFIG diff --git a/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl new file mode 100644 index 00000000..f39e5694 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/pddl/problem_jem_survey.ps2.pddl @@ -0,0 +1,139 @@ +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 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 o0 jem_bay4) (completed-panorama bumble o1 jem_bay3) (completed-panorama bumble o2 jem_bay2) (completed-panorama bumble o3 jem_bay1) (completed-stereo bumble o4 jem_bay1 jem_bay4) (completed-panorama honey o0 jem_bay7) (completed-panorama honey o1 jem_bay6) (completed-panorama honey o2 jem_bay5) (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 (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 (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_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_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_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_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_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_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_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_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 (robot-available bumble) +set predicate (robot-available honey) +set predicate (robot-at bumble berth1) +set predicate (robot-at honey 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 (need-stereo bumble o4 jem_bay1 jem_bay4) +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_manager/survey_planner/readme.md b/astrobee/survey_manager/survey_planner/readme.md new file mode 100644 index 00000000..6fd77ac8 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/readme.md @@ -0,0 +1,5 @@ +\page survey_planner Survey Planner + +Planning and scheduling of queued survey actions using Playsys2 for PDDL solutions and behavior trees for execution. + +Based on [preliminary work](https://github.com/traclabs/astrobee_task_planning_ws) by [Ana](https://github.com/ana-GT) at [Traclabs](https://traclabs.com). diff --git a/astrobee/survey_manager/survey_planner/src/ros1_lifecycle b/astrobee/survey_manager/survey_planner/src/ros1_lifecycle new file mode 160000 index 00000000..39f5ac69 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/ros1_lifecycle @@ -0,0 +1 @@ +Subproject commit 39f5ac693555481d67b9898a0ee172db2692b92d diff --git a/astrobee/survey_manager/survey_planner/src/ros2_planning_system b/astrobee/survey_manager/survey_planner/src/ros2_planning_system new file mode 160000 index 00000000..87d1656d --- /dev/null +++ b/astrobee/survey_manager/survey_planner/src/ros2_planning_system @@ -0,0 +1 @@ +Subproject commit 87d1656d31ebec1eb6c825e601c352cdaffdb95f diff --git a/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py new file mode 100755 index 00000000..73477859 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/plan_interpreter.py @@ -0,0 +1,400 @@ +#!/usr/bin/env python3 + +# 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. + +""" +Provides an example of how to interpret the actions in a survey domain plan, dropping the irrelevant +arguments, mapping the positional arguments back to the names used in the dynamic config, and +looking up extra info that's not needed by the planner but is important for execution (e.g., +coordinates of named locations, fplan filenames of stereo surveys). + +For testing, you can call this from the command line on a complete sample output plan. + +For use during execution, if you are receiving one action at a time from an executor, you may +prefer to import this module and call the yaml_action_from_pddl() function directly. +""" + +import argparse +import collections +import pathlib +import re +import sys +from typing import Any, Dict, Iterable, List, Mapping, Optional, Tuple, TypeVar + +import numpy as np +import yaml +from matplotlib import collections as mc +from matplotlib import patches as mp +from matplotlib import pyplot as plt + +from problem_generator import DATA_DIR, load_yaml, path_list + +DEFAULT_CONFIGS = [ + DATA_DIR / "jem_survey_static.yaml", + # Dynamic config not needed for interpreting the plan +] + +ACTION_TYPE_OPTIONS = ( + "dock", + "undock", + "move", + "panorama", + "stereo", + "let-other-robot-reach", +) +COLORS = {"bumble": "#4080ffff", "honey": "#c0c080ff"} +ROBOTS = list(COLORS.keys()) + +# Type alias +YamlMapping = Dict[str, Any] +FloatStr = str # String representation of a floating-point value +T = TypeVar("T") # pylint: disable=invalid-name + +# POPF plan output weirdly mixes its random debugging output with the actual plan actions that it +# outputs in a standard format. (Maybe there's some way to suppress the debug info?) Anyway, this +# regex works nicely to ignore the debug info and parse the fields of the plan actions we care +# about. It might need fine-tuning if we use other planners. Note: The regex also seems to work ok +# with OPTIC, which was derived from POPF. +PLAN_ACTION_REGEX = re.compile( + r"^(?P\d+(\.\d*)?): (?P\(.*?\))\s+\[(?P\d+(\.\d*)?)\]\s*$" +) + + +class PlanAction: + """ + Class representing one entry in the output plan sequence of a PDDL planner. + """ + + def __init__( + self, start_time_seconds: FloatStr, action: str, duration_seconds: FloatStr + ): + self.start_time_seconds = start_time_seconds + self.action = action + self.duration_seconds = duration_seconds + + def __repr__(self): + return f"{self.start_time_seconds}: {self.action} [{self.duration_seconds}]" + + +def yaml_action_from_pddl( + action: str, static_config: YamlMapping +) -> Optional[YamlMapping]: + """ + Return a YamlMapping representation of `action`. This is the only place + we really need domain-specific logic. + """ + action_args = action[1:-1].split() + action_type = action_args[0] + assert ( + action_type in ACTION_TYPE_OPTIONS + ), f"Expected action type in {ACTION_TYPE_OPTIONS}, got {action_type}" + + if action_type == "dock": + robot, _from_bay, to_berth = action_args[1:] + # Can discard from_bay + return {"type": "dock", "robot": robot, "berth": to_berth} + + if action_type == "undock": + robot, _from_berth, _to_bay, _check1, _check2 = action_args[1:] + # Can discard from_berth, to_bay, check1, check2 + return {"type": "undock", "robot": robot} + + if action_type == "move": + robot, from_bay, to_bay, _check_bay = action_args[1:] + # Can discard check_bay. Look up coordinates for to_bay. + return { + "type": "move", + "robot": robot, + "from_name": from_bay, + "to_name": to_bay, + "to_pos": static_config["bays"][to_bay], + } + + if action_type == "panorama": + robot, _order, location = action_args[1:] + # Can discard order. Look up coordinates for location. + return { + "type": "panorama", + "robot": robot, + "location_name": location, + "location_pos": static_config["bays"][location], + } + + if action_type == "stereo": + robot, _order, base, bound, _check1, _check2 = action_args[1:] + # Use base and bound to look up trajectory. + traj_matches = [ + traj + for traj in static_config["stereo"].values() + if traj["base_location"] == base and traj["bound_location"] == bound + ] + assert ( + len(traj_matches) == 1 + ), f"Expected exactly 1 matching stereo trajectory with base {base} and bound {bound}, got {len(traj_matches)}" + fplan = traj_matches[0]["fplan"] + # Can discard order check1, check2. + return { + "type": "stereo", + "robot": robot, + "fplan": fplan, + "base_name": base, + "bound_name": bound, + } + + if action_type == "let-other-robot-reach": + return None # Action is a no-op intended only to constrain the planner + + assert False, "Never reach this point." + return {} # Make pylint happy + + +def yaml_plan_action_from_pddl( + plan_action: PlanAction, static_config: YamlMapping +) -> Optional[YamlMapping]: + """ + Return a YamlMapping representation of `plan_action`. + """ + action = yaml_action_from_pddl(plan_action.action, static_config) + if action is None: + return None + return { + "start_time_seconds": plan_action.start_time_seconds, + "action": action, + "duration_seconds": plan_action.duration_seconds, + } + + +def parse_plan(plan_path: pathlib.Path) -> List[PlanAction]: + """ + Return a list of PlanActions read from the PDDL plan at `plan_path`. + """ + actions: List[PlanAction] = [] + with plan_path.open(encoding="utf-8") as plan_stream: + for plan_line in plan_stream: + match = PLAN_ACTION_REGEX.search(plan_line) + if match: + start_time_seconds = match.group("start_time_seconds") + action = match.group("action") + duration_seconds = match.group("duration_seconds") + + # Raise an exception if these values are not the string representation of a + # floating-point value. However, we are storing the original string representation + # rather than storing the float to avoid any weird issues with the values getting + # mangled due to floating-point precision and formatting. + float(start_time_seconds) + float(duration_seconds) + + # One of our PDDL planners being evaluated is OPTIC, which is an anytime + # planner. Its overall output potentially includes multiple plans (with increasing + # quality) output during the course of a single planner run. We want to use only the + # last plan. An easy way to do that is to discard all previous actions if we see a + # new action starting at the overall plan start time (t = 0). + if float(start_time_seconds) == 0 and actions: + print( + "WARNING: Found the start of another plan; will use only the last plan", + file=sys.stderr, + ) + actions = [] + + actions.append(PlanAction(start_time_seconds, action, duration_seconds)) + return actions + + +def filter_none(elts: Iterable[Optional[T]]) -> List[T]: + "Return `elts` filtered to remove `None` values." + return [x for x in elts if x is not None] + + +class NoAliasDumper( + yaml.SafeDumper +): # pylint: disable=too-many-ancestors # It only has 1, so (?) + """ + Configures yaml dump output to avoid using confusing aliases. + """ + + def ignore_aliases(self, data): + return True + + +def save_plan_yaml(output_path: pathlib.Path, yaml_actions: List[YamlMapping]) -> None: + "Save plan `yaml_actions` to `output_path` in YAML format." + with output_path.open("w", encoding="utf-8") as output_stream: + yaml.dump(yaml_actions, output_stream, Dumper=NoAliasDumper, sort_keys=False) + print(f"Wrote YAML plan to {output_path}", file=sys.stderr) + + +def plot_plan(plot_path: pathlib.Path, yaml_actions: List[YamlMapping]) -> None: + "Save a plot image of plan `yaml_actions` to `plot_path`." + # Type aliases + TimePos = Tuple[float, int] + PanoLine = Tuple[TimePos, TimePos] + + robot_time_pos: Mapping[str, List[TimePos]] = collections.defaultdict(list) + robot_pano_lines: Mapping[str, List[PanoLine]] = collections.defaultdict(list) + robot_stereo_rects: Mapping[str, List[mp.Rectangle]] = collections.defaultdict(list) + + for plan_action in yaml_actions: + action = plan_action["action"] + robot = action["robot"] + start_time_seconds = float(plan_action["start_time_seconds"]) + duration_seconds = float(plan_action["duration_seconds"]) + end_time_seconds = start_time_seconds + duration_seconds + if action["type"] == "move": + from_bay = int(action["from_name"][-1]) + to_bay = int(action["to_name"][-1]) + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "undock": + from_bay = 8 + to_bay = 7 + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "dock": + from_bay = 7 + to_bay = 8 + robot_time_pos[robot] += [ + (start_time_seconds, from_bay), + (end_time_seconds, to_bay), + ] + elif action["type"] == "panorama": + bay_number = int(action["location_name"][-1]) + robot_pano_lines[robot].append( + ((start_time_seconds, bay_number), (end_time_seconds, bay_number)) + ) + elif action["type"] == "stereo": + base_bay = int(action["base_name"][-1]) + bound_bay = int(action["bound_name"][-1]) + bay1, bay2 = sorted((base_bay, bound_bay)) + padding = 0.5 + robot_stereo_rects[robot].append( + mp.Rectangle( + xy=(start_time_seconds, bay1 - padding), + width=duration_seconds, + height=bay2 - bay1 + 2 * padding, + ) + ) + + fig, ax = plt.subplots() # pylint: disable=invalid-name + for robot in ROBOTS: + time_pos = np.array(robot_time_pos[robot]) + plt.plot(time_pos[:, 0], time_pos[:, 1], "o-", label=robot, color=COLORS[robot]) + + if robot_pano_lines[robot]: + ax.add_collection( + mc.LineCollection( + robot_pano_lines[robot], color=COLORS[robot], linewidth=10 + ) + ) + if robot_stereo_rects[robot]: + ax.add_collection( + mc.PatchCollection( + robot_stereo_rects[robot], facecolor=COLORS[robot], alpha=0.25 + ) + ) + + plt.xlabel("Time (s)") + plt.ylabel("Bay number (dock = 8)") + plt.grid("both") + plt.legend() + fig.set_size_inches((24, 4)) + plt.savefig(plot_path) + print(f"Wrote plot to {plot_path}", file=sys.stderr) + + +def plan_interpreter( + config_paths: Iterable[pathlib.Path], + plan_path: pathlib.Path, + output_path: pathlib.Path, + plot_path: Optional[pathlib.Path], +) -> None: + """ + The main function that interprets an entire plan file. + + However, if you are receiving one action at a time from an executor, you may prefer to import + this module and call yaml_action_from_pddl() directly. + """ + config = {} + for config_path in config_paths: + config.update(load_yaml(config_path)) + pddl_actions = parse_plan(plan_path) + yaml_actions = filter_none( + [ + yaml_plan_action_from_pddl(plan_action, config) + for plan_action in pddl_actions + ] + ) + + save_plan_yaml(output_path, yaml_actions) + + if plot_path is not None: + plot_plan(plot_path, yaml_actions) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + "Custom formatter for argparse that combines mixins." + + +def main(): + "Parse arguments and invoke plan_interpreter()." + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "--config", + help="Comma-separated list of paths to YAML problem config inputs (only static config needed)", + type=path_list, + default=DEFAULT_CONFIGS, + ) + parser.add_argument( + "--plan", + help="Path to input plan generated by PDDL planner (parser currently tuned for POPF idiosyncrasies)", + type=pathlib.Path, + default=DATA_DIR / "sample_output_plan.txt", + ) + parser.add_argument( + "-o", + "--output", + help="Path for output converted to YAML format", + type=pathlib.Path, + default=DATA_DIR / "sample_output_plan.yaml", + ) + parser.add_argument( + "--plot", + help="Write plot to specified path", + type=lambda arg: None if arg is None else pathlib.Path(arg), + default=None, + ) + args = parser.parse_args() + + plan_interpreter( + config_paths=args.config, + plan_path=args.plan, + output_path=args.output, + plot_path=args.plot, + ) + + +if __name__ == "__main__": + main() diff --git a/astrobee/survey_manager/survey_planner/tools/problem_generator.py b/astrobee/survey_manager/survey_planner/tools/problem_generator.py new file mode 100755 index 00000000..72c77327 --- /dev/null +++ b/astrobee/survey_manager/survey_planner/tools/problem_generator.py @@ -0,0 +1,487 @@ +#!/usr/bin/env python3 + +# 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. + +""" +Generates a problem specification for the survey domain in domain_survey.pddl. Output is either a +PDDL problem instance or the equivalent sequence of commands to pass to the PlanSys2 terminal (if +-t is specified). + +Takes as input a list of higher-level configuration files specified in YAML. Normally, this is a +static config that contains non-changing information like bay coordinates, plus a dynamic config +that contains things that are likely to change during execution, like initial state and +goals. However, the split is arbitrary and you can just concatenate all the config files to get the +same effect. + +The generator takes care of error-prone repetitive tasks like asserting predicates for which +locations are different, which robots are different, asserting a need-stereo predicate for every +completed-stereo goal, making the initial location-available predicates agree with the initial robot +locations, etc. +""" + +import argparse +import collections +import io +import itertools +import os +import pathlib +import re +import shlex +import sys +from abc import ABC, abstractmethod +from typing import Any, Dict, Iterable, List, Mapping, Sequence, Tuple, TypeVar + +import yaml + +GOAL_TYPE_OPTIONS = ("panorama", "stereo", "robot_at", "let_other_robot_reach") + +THIS_DIR = pathlib.Path(__file__).resolve().parent +CWD = pathlib.Path.cwd() +DATA_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "data").resolve()), CWD)) +PDDL_DIR = pathlib.Path(os.path.relpath(str((THIS_DIR / ".." / "pddl").resolve()), CWD)) +DEFAULT_CONFIGS = [ + DATA_DIR / "jem_survey_static.yaml", + DATA_DIR / "jem_survey_dynamic.yaml", +] + +# Type alias +YamlMapping = Dict[str, Any] +T = TypeVar("T") # pylint: disable=invalid-name + +# Replace the text "{{ foo }}" in the template with the value of the foo parameter +TEMPLATE_SUBST_REGEX = re.compile(r"{{\s*([\w]+)\s*}}") + + +def load_yaml(yaml_path: pathlib.Path) -> YamlMapping: + """ + Return the YAML parse result for the file at `yaml_path`. + """ + with yaml_path.open(encoding="utf-8") as yaml_stream: + return yaml.safe_load(yaml_stream) + + +def pddl_goal_from_yaml(goal: YamlMapping, config_static: YamlMapping) -> str: + """ + Convert a YAML goal with named fields from the dynamic config into a PDDL goal predicate. + """ + goal_type = goal["type"] + assert ( + goal_type in GOAL_TYPE_OPTIONS + ), f"Expected goal type in {GOAL_TYPE_OPTIONS}, got {goal_type}" + + if goal_type == "panorama": + robot = goal["robot"] + order = goal["order"] + location = goal["location"] + return f"(completed-panorama {robot} o{order} {location})" + + if goal_type == "stereo": + robot = goal["robot"] + order = goal["order"] + trajectory = goal["trajectory"] + traj_info = config_static["stereo"][trajectory] + base = traj_info["base_location"] + bound = traj_info["bound_location"] + return f"(completed-stereo {robot} o{order} {base} {bound})" + + if goal_type == "robot_at": + robot = goal["robot"] + location = goal["location"] + return f"(robot-at {robot} {location})" + + if goal_type == "let_other_robot_reach": + robot = goal["robot"] + location = goal["location"] + order = goal["order"] + return f"(completed-let-other-robot-reach {robot} o{order} {location})" + + assert False, "Never reach this point" + return {} # Make pylint happy + + +def indent_lines(lines: Sequence[str], indent: int) -> str: + """ + Return `lines` joined with carriage returns, prepending `indent` spaces to each line after the + first. + """ + sep = "\n" + (" " * indent) + return sep.join(lines) + + +def pairwise(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: + """ + Returns consecutive pairs drawn from `elts`. Back-port itertools.pairwise() to earlier Python + versions. + """ + a, b = itertools.tee(elts) # pylint: disable=invalid-name + next(b, None) + return zip(a, b) + + +def both_ways(elts: Iterable[Tuple[T, T]]) -> Iterable[Tuple[T, T]]: + """ + Given `elts` an iterable of 2-tuples, return a new iterable that includes each 2-tuple twice, + once in its original order and once reversed. + """ + for a, b in elts: # pylint: disable=invalid-name + yield a, b + yield b, a + + +def distinct_pairs(elts: Iterable[T]) -> Iterable[Tuple[T, T]]: + """ + Return distinct pairs of items drawn from `elts`. + """ + # pylint: disable=invalid-name + for a, b in itertools.product(elts, repeat=2): + if a != b: + yield a, b + + +class TemplateFiller: + """ + Class for substituting parameters into a template. + """ + + def __init__(self, params): + self.params = params + + def __call__(self, match: re.Match) -> str: + param = match.group(1) + if param not in self.params: + raise KeyError( + f"Template param {{{{ {param} }}}} not found in {list(self.params.keys())}" + ) + return self.params[param] + + +def comment_for_pddl(text: str) -> str: + """ + Return the result of commenting `text` using PDDL (Lisp-like) comment syntax. + """ + return "\n".join([f";; {line}".strip() for line in text.splitlines()]) + + +class ProblemWriter(ABC): + "Abstract class for writing a problem intance." + + @abstractmethod + def getvalue(self) -> str: + "Return problem text to output." + + def set_param(self, key: str, value: str) -> None: + "Set template parameter `key` to `value`. (This method is a no-op; override in children.)" + + @abstractmethod + def get_extension(self) -> str: + "Return standard extension to use for output file." + + @abstractmethod + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + "Declare `instance_name` as an instance of `pddl_type`." + + def declare_instances(self, instance_names: Iterable[str], pddl_type: str) -> None: + "Declare `instance_names` as instances of `pddl_type`." + for instance_name in instance_names: + self.declare_instance(instance_name, pddl_type) + + @abstractmethod + def declare_predicate(self, predicate: str, section: str) -> None: + "Declare `predicate` as a PDDL predicate in `section`." + + def declare_predicates(self, predicates: Iterable[str], section: str) -> None: + "Declare `predicates` as PDDL predicates in `section`." + for predicate in predicates: + self.declare_predicate(predicate, section) + + @abstractmethod + def declare_goals(self, goals: Iterable[str]) -> None: + "Declare `goals` as PDDL goals. (Can only be called once, with all goals.)" + + @abstractmethod + def declare_fluent(self, fluent: str, section: str) -> None: + "Declare `fluent` as a PDDL fluent in `section`." + + def declare_fluents(self, fluents: Iterable[str], section: str) -> None: + "Declare `fluents` as PDDL fluents in `section`." + for fluent in fluents: + self.declare_fluent(fluent, section) + + +class PddlWriter(ProblemWriter): + "Class for writing a problem intance in PDDL format." + + def __init__(self, template_path: pathlib.Path): + self.template_path = template_path + self._params: Dict[str, str] = {} + self._section_bufs: Mapping[str, io.StringIO] = collections.defaultdict( + io.StringIO + ) + self._instances_of_types: Mapping[str, List[str]] = collections.defaultdict( + list + ) + + def get_extension(self) -> str: + return ".pddl" + + def set_param(self, key: str, value: str) -> None: + "Set template parameter `key` to `value`." + self._params[key] = value + + def _get_objects_str(self) -> str: + "Return object instance declarations in PDDL format." + indent = " " * 8 + return "\n".join( + ( + f"{indent}{' '.join(instances)} - {pddl_type}" + for pddl_type, instances in self._instances_of_types.items() + ) + ).lstrip() + + def getvalue(self) -> str: + self._params["objects"] = self._get_objects_str() + self._params.update( + { + section: buf.getvalue().strip() + for section, buf in self._section_bufs.items() + } + ) + template_text = self.template_path.read_text() + filled_template = TEMPLATE_SUBST_REGEX.sub( + TemplateFiller(self._params), template_text + ) + return filled_template + + def declare(self, statement: str, section: str) -> None: + "Declare `statement` in `section`." + indent = " " * 8 + self._section_bufs[section].write(indent + statement + "\n") + + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + self._instances_of_types[pddl_type].append(instance_name) + + def declare_predicate(self, predicate: str, section: str) -> None: + self.declare(predicate, section) + + def declare_goals(self, goals: Iterable[str]) -> None: + indent = " " * 12 + self.set_param("goals", "\n".join((indent + goal for goal in goals)).lstrip()) + + def declare_fluent(self, fluent: str, section: str) -> None: + self.declare(fluent, section) + + +class TerminalWriter(ProblemWriter): + "Class for writing a problem instance as a sequence of PlanSys2 terminal commands." + + def __init__(self): + self._buf = io.StringIO() + + def get_extension(self) -> str: + return ".ps2.pddl" + + def getvalue(self) -> str: + return self._buf.getvalue() + + def declare(self, statement: str) -> None: + "Declare `statement`." + self._buf.write(statement + "\n") + + def declare_instance(self, instance_name: str, pddl_type: str) -> None: + self.declare(f"set instance {instance_name} {pddl_type}") + + def declare_predicate(self, predicate: str, section: str) -> None: + self.declare(f"set predicate {predicate}") + + def declare_goals(self, goals: Iterable[str]) -> None: + self.declare(f"set goal (and {' '.join(goals)})") + + def declare_fluent(self, fluent: str, section: str) -> None: + self.declare(f"set function {fluent}") + + +def problem_generator( + problem_template_path: pathlib.Path, + config_paths: Iterable[pathlib.Path], + output_path_template: pathlib.Path, + command: str, + terminal: bool, +) -> None: + """ + The main function that generates the problem. + """ + config = {} + for config_path in config_paths: + config.update(load_yaml(config_path)) + + if terminal: + writer: ProblemWriter = TerminalWriter() + else: + writer = PddlWriter(problem_template_path) + header_lines = ( + ";; Auto-generated by problem_generator.py. Do not edit!\n" + f";; Command was: {command}\n" + f";; Working directory was: {CWD}\n" + f";; Problem template: {problem_template_path}\n" + ) + full_config = "" + for i, config_path in enumerate(config_paths): + header_lines += f";; Config {i + 1}: {config_path}\n" + full_config += config_path.read_text() + writer.set_param("header", header_lines) + writer.set_param("config", comment_for_pddl(full_config)) + + bays = list(config["bays"].keys()) + bogus_bays = config["bogus_bays"] + all_bays = sorted(bays + bogus_bays) + writer.declare_instances(all_bays, "location") + + berths = config["berths"] + writer.declare_instances(berths, "location") + + robots = config["robots"] + writer.declare_instances(robots, "robot") + + max_order = max([goal.get("order", -1) for goal in config["goals"]]) + num_orders = max_order + 1 + orders = [f"o{i}" for i in range(num_orders)] + writer.declare_instances(orders, "order") + + yaml_goals = config["goals"] + pddl_goals = [pddl_goal_from_yaml(goal, config) for goal in yaml_goals] + writer.declare_goals(pddl_goals) + + move_connected_lines = [ + f"(move-connected {a} {b})" for a, b in both_ways(pairwise(all_bays)) + ] + writer.declare_predicates(move_connected_lines, "static_predicates") + + location_real_lines = [f"(location-real {bay})" for bay in bays] + writer.declare_predicates(location_real_lines, "static_predicates") + + candidates = (("jem_bay7", "berth1"), ("jem_bay7", "berth2")) + dock_connected_lines = [ + f"(dock-connected {bay} {berth})" + for bay, berth in candidates + if bay in bays and berth in berths + ] + writer.declare_predicates(dock_connected_lines, "static_predicates") + + robots_different_lines = [ + f"(robots-different {a} {b})" for a, b in distinct_pairs(robots) + ] + writer.declare_predicates(robots_different_lines, "static_predicates") + + locs_different_lines = [ + f"(locations-different {a} {b})" for a, b in distinct_pairs(all_bays) + ] + writer.declare_predicates(locs_different_lines, "static_predicates") + + robot_available_lines = [f"(robot-available {robot})" for robot in robots] + writer.declare_predicates(robot_available_lines, "dynamic_predicates") + + init = config["init"] + robot_at_lines = [ + f"(robot-at {robot} {init[robot]['location']})" for robot in robots + ] + writer.declare_predicates(robot_at_lines, "dynamic_predicates") + + all_locations = all_bays + berths + occupied_locations = [init[robot]["location"] for robot in robots] + available_locations = sorted(set(all_locations).difference(occupied_locations)) + location_available_lines = [ + f"(location-available {location})" for location in available_locations + ] + writer.declare_predicates(location_available_lines, "dynamic_predicates") + + need_stereo_lines = [ + goal.replace("completed-stereo", "need-stereo") + for goal in pddl_goals + if "completed-stereo" in goal + ] + writer.declare_predicates(need_stereo_lines, "dynamic_predicates") + + order_identity_lines = [f"(= (order-identity o{i}) {i})" for i in range(num_orders)] + writer.declare_fluents(order_identity_lines, "static_fluents") + + robot_order_lines = [f"(= (robot-order {robot}) -1)" for robot in robots] + writer.declare_fluents(robot_order_lines, "dynamic_fluents") + + output_path = pathlib.Path( + str(output_path_template).replace("{ext}", writer.get_extension()) + ) + output_path.write_text(writer.getvalue()) + print(f"Wrote to {output_path}", file=sys.stderr) + + +class CustomFormatter( + argparse.ArgumentDefaultsHelpFormatter, argparse.RawDescriptionHelpFormatter +): + "Custom formatter for argparse that combines mixins." + + +def path_list(paths_text: str) -> List[pathlib.Path]: + "Return the list of paths parsed from comma-separated list `paths_text`." + return [pathlib.Path(pstr) for pstr in paths_text.split(",")] + + +def main(): + "Parse arguments and invoke problem_generator()." + parser = argparse.ArgumentParser( + description=__doc__, formatter_class=CustomFormatter + ) + parser.add_argument( + "-t", + "--terminal", + help="Format output for PlanSys2 terminal instead of PDDL", + action="store_true", + default=False, + ) + parser.add_argument( + "--problem-template", + help="Path to input PDDL problem template (unused if -t specified)", + type=pathlib.Path, + default=PDDL_DIR / "jem_survey_template.pddl", + ) + parser.add_argument( + "--config", + help="Comma-separated list of paths to YAML problem config inputs", + type=path_list, + default=",".join([str(path) for path in DEFAULT_CONFIGS]), + ) + parser.add_argument( + "-o", + "--output", + help="Path for output PDDL problem ({ext} filled with extension)", + type=pathlib.Path, + default=PDDL_DIR / "problem_jem_survey{ext}", + ) + args = parser.parse_args() + + problem_generator( + problem_template_path=args.problem_template, + config_paths=args.config, + output_path_template=args.output, + command=shlex.join(sys.argv), + terminal=args.terminal, + ) + + +if __name__ == "__main__": + main() diff --git a/isaac/Subsystems.md b/isaac/Subsystems.md index bff1f708..ddd1e7a8 100644 --- a/isaac/Subsystems.md +++ b/isaac/Subsystems.md @@ -13,4 +13,6 @@ The ISAAC Repo contains the following Modules: \subpage astrobee : High-level actions that support complex maneuvers. +\subpage survey : Semi-autonomous survey tasking of Astrobees. + \subpage shared : Shared tools common to the entire ISAAC repo. diff --git a/scripts/docker/astrobee_msgs.Dockerfile b/scripts/docker/astrobee_msgs.Dockerfile index 1ac4e76c..1a8e4716 100644 --- a/scripts/docker/astrobee_msgs.Dockerfile +++ b/scripts/docker/astrobee_msgs.Dockerfile @@ -19,11 +19,11 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 FROM ubuntu:${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # try to suppress certain warnings during apt-get calls ARG DEBIAN_FRONTEND=noninteractive diff --git a/scripts/docker/isaac.Dockerfile b/scripts/docker/isaac.Dockerfile index 18fb3cd3..4ee67c98 100644 --- a/scripts/docker/isaac.Dockerfile +++ b/scripts/docker/isaac.Dockerfile @@ -19,12 +19,12 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=isaac FROM ${REMOTE}/isaac:latest-astrobee-ubuntu${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # suppress detached head warnings later RUN git config --global advice.detachedHead false diff --git a/scripts/docker/isaac_astrobee.Dockerfile b/scripts/docker/isaac_astrobee.Dockerfile index 269c85ca..12afb06d 100644 --- a/scripts/docker/isaac_astrobee.Dockerfile +++ b/scripts/docker/isaac_astrobee.Dockerfile @@ -19,13 +19,13 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=astrobee FROM ${REMOTE}/astrobee:latest-ubuntu${UBUNTU_VERSION} # Already inherited from astrobee:base-latest-ubuntu... -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 RUN apt-get update && apt-get install -y \ libmnl-dev \ diff --git a/scripts/docker/isaac_msgs.Dockerfile b/scripts/docker/isaac_msgs.Dockerfile index 27190252..280c5ae8 100644 --- a/scripts/docker/isaac_msgs.Dockerfile +++ b/scripts/docker/isaac_msgs.Dockerfile @@ -19,12 +19,12 @@ # This will set up an Astrobee docker container using the non-NASA install instructions. # You must set the docker context to be the repository root directory -ARG UBUNTU_VERSION=16.04 +ARG UBUNTU_VERSION=20.04 ARG REMOTE=isaac FROM ${REMOTE}/isaac:astrobee-msgs-ubuntu${UBUNTU_VERSION} -ARG ROS_VERSION=kinetic -ARG PYTHON="" +ARG ROS_VERSION=noetic +ARG PYTHON=3 # Copy over the isaac_msgs COPY communications/isaac_msgs /src/msgs/src/communications/ diff --git a/scripts/setup/packages_focal.lst b/scripts/setup/packages_focal.lst index 87954901..e1c70e58 100644 --- a/scripts/setup/packages_focal.lst +++ b/scripts/setup/packages_focal.lst @@ -2,7 +2,9 @@ doxygen python3-catkin-tools python3-osrf-pycommon python3-rosdep +libreadline-dev ros-noetic-eigen-conversions ros-noetic-pcl-ros +ros-noetic-behaviortree-cpp libmnl-dev libproj-dev