From 5c674d746a9bb6a15dda8402e0fbca29b56d062f Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Mon, 28 Feb 2022 08:53:55 -0800 Subject: [PATCH 1/6] Anomaly detection/ Analyst notebook architecture + start (#36) * adding anomaly classes and new folders * adding readme of new packages * folder struct change * unify scripts; add remote images; better documentation * database connecting + analyst dockerfile with installed packages * fix scicam commend in get_train_data * progress on analyst notebook setup * working analyst notebook connection * add analyst notebook image to CI pipeline --- .github/workflows/ci_pr.yml | 6 +- .github/workflows/ci_push.yml | 7 + .gitignore | 9 +- DEMO_INSTALL.md | 40 ---- INSTALL.md | 6 +- analyst/readme.md | 4 + analyst/workspace/1_import_bagfiles.ipynb | 105 +++++++++ analyst/workspace/2_read_from_database.ipynb | 49 +++++ .../workspace/build_CNN_with_pytorch.ipynb | 70 ++++++ .../workspace/scripts/load_bag_database.py | 87 ++++++++ .../image}/CMakeLists.txt | 0 .../image}/include/img_analysis/img_vent.h | 0 .../image}/launch/img_analysis.launch | 0 .../image}/nodelet_plugins.xml | 0 {img_analysis => anomaly/image}/package.xml | 0 {img_analysis => anomaly/image}/readme.md | 0 .../image}/resources/model_cnn.pt | Bin .../image}/resources/other_poses | 0 .../image}/resources/vent_poses | 0 .../image}/resources/vent_traced_model_cnn.pt | Bin .../image}/scripts/analyse_img.py | 0 .../image}/scripts/train_cnn_vent.py | 0 .../image}/src/img_analysis_nodelet.cc | 0 .../image}/src/img_vent.cc | 0 .../image}/test/test_vent.cc | 0 .../image}/test/test_vent.test | 0 .../image}/tools/get_train_data_vent.cc | 2 +- anomaly/readme.md | 10 + anomaly/semantic/readme.md | 3 + anomaly/signal/readme.md | 3 + anomaly/volumetric/readme.md | 3 + isaac.doxyfile | 2 +- isaac/Subsystems.md | 4 +- scripts/docker/analyst.Dockerfile | 44 ++++ scripts/docker/astrobee_msgs.Dockerfile | 2 +- scripts/docker/build.sh | 52 ++--- .../docker_compose/analyst.docker-compose.yml | 27 +++ .../docker_compose/idi.docker-compose.yml | 121 ++--------- scripts/docker/readme.md | 80 ++++++- scripts/docker/run.sh | 201 +++++++++++++++--- scripts/docker/run_ground.sh | 84 -------- scripts/docker/run_vm.sh | 80 ------- scripts/docker/shutdown.sh | 14 +- 43 files changed, 737 insertions(+), 378 deletions(-) delete mode 100644 DEMO_INSTALL.md create mode 100644 analyst/readme.md create mode 100644 analyst/workspace/1_import_bagfiles.ipynb create mode 100644 analyst/workspace/2_read_from_database.ipynb create mode 100644 analyst/workspace/build_CNN_with_pytorch.ipynb create mode 100644 analyst/workspace/scripts/load_bag_database.py rename {img_analysis => anomaly/image}/CMakeLists.txt (100%) rename {img_analysis => anomaly/image}/include/img_analysis/img_vent.h (100%) rename {img_analysis => anomaly/image}/launch/img_analysis.launch (100%) rename {img_analysis => anomaly/image}/nodelet_plugins.xml (100%) rename {img_analysis => anomaly/image}/package.xml (100%) rename {img_analysis => anomaly/image}/readme.md (100%) rename {img_analysis => anomaly/image}/resources/model_cnn.pt (100%) rename {img_analysis => anomaly/image}/resources/other_poses (100%) rename {img_analysis => anomaly/image}/resources/vent_poses (100%) rename {img_analysis => anomaly/image}/resources/vent_traced_model_cnn.pt (100%) rename {img_analysis => anomaly/image}/scripts/analyse_img.py (100%) rename {img_analysis => anomaly/image}/scripts/train_cnn_vent.py (100%) rename {img_analysis => anomaly/image}/src/img_analysis_nodelet.cc (100%) rename {img_analysis => anomaly/image}/src/img_vent.cc (100%) rename {img_analysis => anomaly/image}/test/test_vent.cc (100%) rename {img_analysis => anomaly/image}/test/test_vent.test (100%) rename {img_analysis => anomaly/image}/tools/get_train_data_vent.cc (99%) create mode 100644 anomaly/readme.md create mode 100644 anomaly/semantic/readme.md create mode 100644 anomaly/signal/readme.md create mode 100644 anomaly/volumetric/readme.md create mode 100644 scripts/docker/analyst.Dockerfile create mode 100644 scripts/docker/docker_compose/analyst.docker-compose.yml delete mode 100755 scripts/docker/run_ground.sh delete mode 100755 scripts/docker/run_vm.sh diff --git a/.github/workflows/ci_pr.yml b/.github/workflows/ci_pr.yml index ddf03286..ebc9ada8 100644 --- a/.github/workflows/ci_pr.yml +++ b/.github/workflows/ci_pr.yml @@ -142,4 +142,8 @@ jobs: --build-arg UBUNTU_VERSION=20.04 --build-arg ROS_VERSION=noetic --build-arg PYTHON=3 - -t isaac/isaac:msgs-ubuntu20.04 \ No newline at end of file + -t isaac/isaac:msgs-ubuntu20.04 + + - name: Build analyst image isaac/isaac:msgs-ubuntu20.04 + run: docker build isaac -f isaac/scripts/docker/analyst.Dockerfile + -t isaac/isaac_analyst_notebook:latest \ No newline at end of file diff --git a/.github/workflows/ci_push.yml b/.github/workflows/ci_push.yml index 60b58a05..b9914462 100644 --- a/.github/workflows/ci_push.yml +++ b/.github/workflows/ci_push.yml @@ -173,6 +173,12 @@ jobs: --build-arg REMOTE=ghcr.io/nasa -t ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu20.04 + - name: Build analyst image isaac/isaac:msgs-ubuntu20.04 + run: docker build isaac -f isaac/scripts/docker/analyst.Dockerfile + --build-arg REMOTE=ghcr.io/nasa + -t ghcr.io/${{ github.repository_owner }}/isaac_analyst_notebook:latest + + - name: Log in to registry run: echo "${{ secrets.GITHUB_TOKEN }}" | docker login ghcr.io -u ${{ github.repository_owner }} --password-stdin @@ -182,3 +188,4 @@ jobs: if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:latest-ubuntu20.04; fi; if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:astrobee-msgs-ubuntu20.04; fi; if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac:msgs-ubuntu20.04; fi; + if [ "${{ github.repository_owner }}" = "nasa" ]; then docker push ghcr.io/${{ github.repository_owner }}/isaac_analyst_notebook:latest; fi; diff --git a/.gitignore b/.gitignore index b5e1e020..46089f6b 100644 --- a/.gitignore +++ b/.gitignore @@ -46,4 +46,11 @@ tags # Folders doc/html -doc/latex \ No newline at end of file +doc/latex + +# Jupyter Notebooks +analyst/workspace/.ipython +analyst/workspace/.jupyter +analyst/workspace/.local +analyst/workspace/.ipynb_checkpoints +analyst/workspace/.cache \ No newline at end of file diff --git a/DEMO_INSTALL.md b/DEMO_INSTALL.md deleted file mode 100644 index 5c4e0439..00000000 --- a/DEMO_INSTALL.md +++ /dev/null @@ -1,40 +0,0 @@ -Demo docker install -===== - -Instructions for the full demo install - -Check out ---------- - -Run: - - mkdir ~/ws - cd ~/ws - git clone ssh://git@babelfish.arc.nasa.gov:7999/isaac/isaac.git --branch develop - ./isaac/scripts/checkout.sh - # if you prefer to use [vcstool](https://github.com/dirk-thomas/vcstool), you can run this checkout script instead: - ./isaac/scripts/checkout-vcstool.sh - -(You can also modify the checkout location `~/ws` if you want.) - -Install dependencies ---------- - -Install docker tools: - - sudo apt-get install docker.io docker-compose - -Install nvidia-docker by following (the directions here)[https://github.com/NVIDIA/nvidia-docker]. - -Build ---------- - -Run `scripts/docker/build.sh` to build the docker images for the demo. - -Install ---------- - -Run `scripts/docker/run.sh` to run the demo. Open `http://127.0.0.1:8080` in a web browser to see what is happening. Use -`docker ps` to see the docker containers and use `docker exec -it container_name /bin/bash` to get a shell in one. - -Cancel with Ctrl+c and then run `scripts/docker/shutdown.sh` to stop the demo. diff --git a/INSTALL.md b/INSTALL.md index d8561607..b71c1f2b 100644 --- a/INSTALL.md +++ b/INSTALL.md @@ -1,7 +1,7 @@ Native Install ===== -Usage instructions for non-NASA and NASA users +Machine setup --------- Install the 64-bit version of [Ubuntu 16.04, 18.04 or 20.04](http://releases.ubuntu.com/) @@ -18,10 +18,8 @@ any other operating system or Ubuntu versions.* *Note: Please ensure you install the 64-bit version of Ubuntu. We do not support running ISAAC Software on 32-bit systems.* -Machine setup ---------- +**The `isaac` repo depends on some `astrobee` packages, therefore, `astrobee` needs to be installed beforehand.** -The `isaac` repo depends on some `astrobee` packages, therefore, `astrobee` needs to be installed beforehand. Checkout the project source code --------- diff --git a/analyst/readme.md b/analyst/readme.md new file mode 100644 index 00000000..014bdfa5 --- /dev/null +++ b/analyst/readme.md @@ -0,0 +1,4 @@ +\page analyst Analyst Notebook + + +copied into the jupyter notebook home folder, are scripts that are made by the analyst and will be saved, even after the docker is shutdown. \ No newline at end of file diff --git a/analyst/workspace/1_import_bagfiles.ipynb b/analyst/workspace/1_import_bagfiles.ipynb new file mode 100644 index 00000000..20581b1b --- /dev/null +++ b/analyst/workspace/1_import_bagfiles.ipynb @@ -0,0 +1,105 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "metadata": { + "tags": [] + }, + "source": [ + "# Import bagfiles to Database for Analysis\n", + "\n", + "The database used is ArangoDB. If you followed the Analyst notebook startup procedure [here](https://nasa.github.io/isaac/html/index.html), the local database should already be hosted in http://localhost:8529.\n", + "\n", + "Go ahead open the link at take a look! The username is `root` and the password `isaac`.\n", + "\n", + "To connect to the database:" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from pyArango.connection import *\n", + "\n", + "# Connect to the database\n", + "conn = Connection(arangoURL=\"http://iui_arangodb:8529\", username=\"root\", password=\"isaac\")\n", + "\n", + "# Open the isaac database / create it if it does not exist\n", + "if not conn.hasDatabase(\"isaac\"):\n", + " conn.createDatabase(name=\"isaac\")\n", + "db = conn[\"isaac\"]\n", + "\n", + "# Create a collection\n", + "if not db.hasCollection(\"analyst\"):\n", + " db.createCollection(name=\"analyst\")\n", + " \n", + "print(\"Connected to database!\")" + ] + }, + { + "cell_type": "markdown", + "metadata": {}, + "source": [ + "The next step is to select the bagfile to import" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [ + "from scripts.load_bag_database import LoadBagDatabase\n", + "\n", + "\n", + "path=\"/home/analyst/data/bags/\"\n", + "\n", + "LoadBagDatabase(db, path, \"/gnc/ekf\")" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 4 +} diff --git a/analyst/workspace/2_read_from_database.ipynb b/analyst/workspace/2_read_from_database.ipynb new file mode 100644 index 00000000..b00161c5 --- /dev/null +++ b/analyst/workspace/2_read_from_database.ipynb @@ -0,0 +1,49 @@ +{ + "cells": [ + { + "cell_type": "markdown", + "id": "5636fde2-c963-48b5-a883-a7d520a6084e", + "metadata": {}, + "source": [ + "# Read Various Data Types from Database" + ] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "23334b94-b585-4319-a75b-eeacece22852", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9288a825-64f6-4177-9f65-9bff1ea74210", + "metadata": {}, + "outputs": [], + "source": [] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.8.10" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/analyst/workspace/build_CNN_with_pytorch.ipynb b/analyst/workspace/build_CNN_with_pytorch.ipynb new file mode 100644 index 00000000..71f4a411 --- /dev/null +++ b/analyst/workspace/build_CNN_with_pytorch.ipynb @@ -0,0 +1,70 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": null, + "id": "1b6a6b4d-99b1-47ca-8cfd-a668539fdab8", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": null, + "id": "9df2443f-50cc-49a9-9b4e-2f1018fc6904", + "metadata": {}, + "outputs": [], + "source": [] + }, + { + "cell_type": "code", + "execution_count": 1, + "id": "097ce74d-b561-4aa4-b899-8301205457a2", + "metadata": {}, + "outputs": [], + "source": [ + "from pathlib import Path\n", + "\n", + "import matplotlib.pyplot as plt\n", + "import numpy as np\n", + "import torch\n", + "import torch.nn.functional as F\n", + "import torchvision.transforms as transforms\n", + "from PIL import Image\n", + "from torch import nn, optim\n", + "from torchvision import datasets, models, transforms\n", + "\n", + "\n", + "\n", + "test_transforms = transforms.Compose(\n", + " [\n", + " transforms.Resize(224),\n", + " transforms.ToTensor(),\n", + " transforms.Normalize([0.485, 0.456, 0.406], [0.229, 0.224, 0.225]),\n", + " ]\n", + ")" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "Python 3 (ipykernel)", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.9.7" + } + }, + "nbformat": 4, + "nbformat_minor": 5 +} diff --git a/analyst/workspace/scripts/load_bag_database.py b/analyst/workspace/scripts/load_bag_database.py new file mode 100644 index 00000000..d46477f7 --- /dev/null +++ b/analyst/workspace/scripts/load_bag_database.py @@ -0,0 +1,87 @@ +# Copyright © 2021, 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. +# +# ---------------------------------------------------------------------------------------------------- +# ISAAC Interface +# Backend API +# ---------------------------------------------------------------------------------------------------- +# Database interface class definition +# ---------------------------------------------------------------------------------------------------- + + +# roslibpy needs a logger in order to output errors inside callbacks +import logging +import sys +import time +from os import listdir +from os.path import isfile, join + +import rosbag +import yaml +from pyArango.connection import * + +logging.basicConfig() + + +class LoadBagDatabase: + def __init__(self, database, path, topics): + self.db = database + # TODO Marina: Recursively look into bag directory + # Check the folder contents + bagfiles = [f for f in listdir(path) if f.endswith(".bag")] + print("bagfiles" + str(bagfiles) + "\ntopics" + str(topics)) + for bag in bagfiles: + self.read_bag(path + bag, topics) + + # TODO Marina: automatically joint bags split into different parts + def read_bag(self, bag_file, topics_list): + # access bag + print("Reading bag file") + bag = rosbag.Bag(bag_file) + bag_contents = bag.read_messages() + bagName = bag.filename + + # get list of topics from the bag + bag_topics = [] + for topic, msg, t in bag_contents: + if topic in topics_list: + bag_topics.append(topic) + print("List of topics " + str(bag_topics)) + + # create a new collection with the bag name + # collection name needs to start with a letter + bagFile = "bag_" + bagFile[:-4] + print("Creating collection " + bagFile) + if not self.db.hasCollection("yo"): + self.db.createCollection(name="yo") + # ensure index + self.db["yo"].ensureSkiplistIndex(["header.stamp.secs"]) + + # open + for topic_name in bag_topics: + # Go through all the messages in a topic + for subtopic, msg, t in bag.read_messages(topic_name): + msg = yaml.safe_load(str(msg)) + + aql = ( + "INSERT " + + str(msg) + + " INTO " + + "yo" + + " LET newDoc = NEW RETURN newDoc" + ) + queryResult = self.db.AQLQuery(aql) + bag.close() diff --git a/img_analysis/CMakeLists.txt b/anomaly/image/CMakeLists.txt similarity index 100% rename from img_analysis/CMakeLists.txt rename to anomaly/image/CMakeLists.txt diff --git a/img_analysis/include/img_analysis/img_vent.h b/anomaly/image/include/img_analysis/img_vent.h similarity index 100% rename from img_analysis/include/img_analysis/img_vent.h rename to anomaly/image/include/img_analysis/img_vent.h diff --git a/img_analysis/launch/img_analysis.launch b/anomaly/image/launch/img_analysis.launch similarity index 100% rename from img_analysis/launch/img_analysis.launch rename to anomaly/image/launch/img_analysis.launch diff --git a/img_analysis/nodelet_plugins.xml b/anomaly/image/nodelet_plugins.xml similarity index 100% rename from img_analysis/nodelet_plugins.xml rename to anomaly/image/nodelet_plugins.xml diff --git a/img_analysis/package.xml b/anomaly/image/package.xml similarity index 100% rename from img_analysis/package.xml rename to anomaly/image/package.xml diff --git a/img_analysis/readme.md b/anomaly/image/readme.md similarity index 100% rename from img_analysis/readme.md rename to anomaly/image/readme.md diff --git a/img_analysis/resources/model_cnn.pt b/anomaly/image/resources/model_cnn.pt similarity index 100% rename from img_analysis/resources/model_cnn.pt rename to anomaly/image/resources/model_cnn.pt diff --git a/img_analysis/resources/other_poses b/anomaly/image/resources/other_poses similarity index 100% rename from img_analysis/resources/other_poses rename to anomaly/image/resources/other_poses diff --git a/img_analysis/resources/vent_poses b/anomaly/image/resources/vent_poses similarity index 100% rename from img_analysis/resources/vent_poses rename to anomaly/image/resources/vent_poses diff --git a/img_analysis/resources/vent_traced_model_cnn.pt b/anomaly/image/resources/vent_traced_model_cnn.pt similarity index 100% rename from img_analysis/resources/vent_traced_model_cnn.pt rename to anomaly/image/resources/vent_traced_model_cnn.pt diff --git a/img_analysis/scripts/analyse_img.py b/anomaly/image/scripts/analyse_img.py similarity index 100% rename from img_analysis/scripts/analyse_img.py rename to anomaly/image/scripts/analyse_img.py diff --git a/img_analysis/scripts/train_cnn_vent.py b/anomaly/image/scripts/train_cnn_vent.py similarity index 100% rename from img_analysis/scripts/train_cnn_vent.py rename to anomaly/image/scripts/train_cnn_vent.py diff --git a/img_analysis/src/img_analysis_nodelet.cc b/anomaly/image/src/img_analysis_nodelet.cc similarity index 100% rename from img_analysis/src/img_analysis_nodelet.cc rename to anomaly/image/src/img_analysis_nodelet.cc diff --git a/img_analysis/src/img_vent.cc b/anomaly/image/src/img_vent.cc similarity index 100% rename from img_analysis/src/img_vent.cc rename to anomaly/image/src/img_vent.cc diff --git a/img_analysis/test/test_vent.cc b/anomaly/image/test/test_vent.cc similarity index 100% rename from img_analysis/test/test_vent.cc rename to anomaly/image/test/test_vent.cc diff --git a/img_analysis/test/test_vent.test b/anomaly/image/test/test_vent.test similarity index 100% rename from img_analysis/test/test_vent.test rename to anomaly/image/test/test_vent.test diff --git a/img_analysis/tools/get_train_data_vent.cc b/anomaly/image/tools/get_train_data_vent.cc similarity index 99% rename from img_analysis/tools/get_train_data_vent.cc rename to anomaly/image/tools/get_train_data_vent.cc index e507fb2a..04301b6f 100644 --- a/img_analysis/tools/get_train_data_vent.cc +++ b/anomaly/image/tools/get_train_data_vent.cc @@ -258,7 +258,7 @@ int ReadFile() { // Test ChangePoses(vent_transform); - PublishSciCamCommand("takeSinglePicture"); + PublishSciCamCommand("takePicture"); ros::spinOnce(); } return 0; diff --git a/anomaly/readme.md b/anomaly/readme.md new file mode 100644 index 00000000..5f86a5ca --- /dev/null +++ b/anomaly/readme.md @@ -0,0 +1,10 @@ +\page ano Anomaly Detectors + +Anomaly Detectors +==================== + + +\subpage signal_anomaly +\subpage semantic_anomaly +\subpage image_anomaly +\subpage volumetric_anomaly diff --git a/anomaly/semantic/readme.md b/anomaly/semantic/readme.md new file mode 100644 index 00000000..d9ee8d4d --- /dev/null +++ b/anomaly/semantic/readme.md @@ -0,0 +1,3 @@ +\page semantic_anomaly Semantic + +Semantic anomaly detection documentation \ No newline at end of file diff --git a/anomaly/signal/readme.md b/anomaly/signal/readme.md new file mode 100644 index 00000000..8b77ff6f --- /dev/null +++ b/anomaly/signal/readme.md @@ -0,0 +1,3 @@ +\page signal_anomaly Signal + +Signal anomaly detection documentation \ No newline at end of file diff --git a/anomaly/volumetric/readme.md b/anomaly/volumetric/readme.md new file mode 100644 index 00000000..9b2a3c9d --- /dev/null +++ b/anomaly/volumetric/readme.md @@ -0,0 +1,3 @@ +\page volumetric_anomaly Volumetric + +Volumetric anomaly detection documentation \ No newline at end of file diff --git a/isaac.doxyfile b/isaac.doxyfile index ad6b0fc5..6842160e 100644 --- a/isaac.doxyfile +++ b/isaac.doxyfile @@ -836,7 +836,7 @@ WARN_LOGFILE = INPUT = README.md \ INSTALL.md \ - DEMO_INSTALL.md \ + scripts \ isaac \ astrobee \ dense_map \ diff --git a/isaac/Subsystems.md b/isaac/Subsystems.md index 4f327357..bff1f708 100644 --- a/isaac/Subsystems.md +++ b/isaac/Subsystems.md @@ -7,7 +7,9 @@ The ISAAC Repo contains the following Modules: \subpage idm : Tools to build the different layers of the dense map. -\subpage ano : Analisys tools for inspection of the ISS environment. +\subpage ano : Analysis tools for inspection of the ISS environment. + +\subpage analyst : Analyst Notebook for data processing. \subpage astrobee : High-level actions that support complex maneuvers. diff --git a/scripts/docker/analyst.Dockerfile b/scripts/docker/analyst.Dockerfile new file mode 100644 index 00000000..2a878840 --- /dev/null +++ b/scripts/docker/analyst.Dockerfile @@ -0,0 +1,44 @@ +# Copyright (c) 2021, 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. + +# 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 REMOTE=isaac +FROM ${REMOTE}/isaac:msgs-ubuntu20.04 + +RUN apt-get update \ + && apt-get install -y python3-pip \ + && rm -rf /var/lib/apt/lists/* + +RUN pip3 install pyArango \ + && pip3 install torch==1.10.2+cpu torchvision==0.11.3+cpu -f https://download.pytorch.org/whl/cpu/torch_stable.html \ + && pip3 install jupyterlab jupyterhub nbconvert Pygments==2.6.1 + +EXPOSE 8888 + +# Setup work directory and add the entrypoint for docker +RUN mkdir /home/analyst \ + && echo "\ncd /home/analyst\njupyter lab --allow-root --no-browser --ip 0.0.0.0\n" >> /ros_entrypoint.sh \ + && cat /ros_entrypoint.sh + + +# Configure container startup +#ENTRYPOINT ["/usr/bin/bash", "/ros_entrypoint.sh"] +#CMD ["ros_entrypoint.sh"] diff --git a/scripts/docker/astrobee_msgs.Dockerfile b/scripts/docker/astrobee_msgs.Dockerfile index 33fb38f7..1ac4e76c 100644 --- a/scripts/docker/astrobee_msgs.Dockerfile +++ b/scripts/docker/astrobee_msgs.Dockerfile @@ -54,7 +54,7 @@ RUN apt-get update \ python${PYTHON}-catkin-tools \ && rm -rf /var/lib/apt/lists/* -#Add the entrypoint for docker +# Add the entrypoint for docker RUN echo "#!/bin/bash\nset -e\n\nsource \"/opt/ros/${ROS_VERSION}/setup.bash\"\nexec \"\$@\"" > /ros_entrypoint.sh && \ chmod +x /ros_entrypoint.sh && \ rosdep init && \ diff --git a/scripts/docker/build.sh b/scripts/docker/build.sh index aadffe94..80542ca9 100755 --- a/scripts/docker/build.sh +++ b/scripts/docker/build.sh @@ -21,33 +21,35 @@ # Exit immediately if a command exits with a non-zero status set -e -# short help -usage_string="$scriptname [-h] [-x ]\ - [-b ] [-f ]\ - [-a ] [-d ]\ - [-m ] [-n ] [-v ]" -#[-t make_target] +# Print the help message (list all the options) +print_help() +{ + echo -e "Builds docker images for the ISAAC stack" + echo -e "The build script will automatically detect the current Ubuntu OS version and" + echo -e "define the docker files variables UBUNTU_VERSION, ROS_VERSION, and PYTHON accordingly." + echo -e "Options:" + echo -e "\t-x | --xenial\t\t\tBuild images for Ubuntu 16.04" + echo -e "\t-b | --bionic\t\t\tBuild images for Ubuntu 18.04" + echo -e "\t-f | --focal\t\t\tBuild images for Ubuntu 20.04" + echo -e "\t-a | --astrobee-source-path\tSpecify the astrobee source directory to use" + echo -e "\t\t\t\tdefault=isaac_source/../../astrobee" + echo -e "\t-i | --iui-source-dir\t\tSpecify the idi source directory to use" + echo -e "\t\t\t\tdefault=isaac_source/../../isaac_data_interface" + echo -e "\t-m | --mast-source-dir\t\tSpecify the mast source directory to use" + echo -e "\t\t\t\tdefault=isaac_source/../../mast/src" + echo -e "\t-n | --no-mast\t\t\tBuild without MAST" + echo -e "\t-v | --vm\t\t\tBuild images compatible with virtual machine only" + echo +} # Print the script usage print_usage() { - echo $usage_string + echo -e "Invalid option!\n\n" + print_help } -# Print the help message (list all the options) -print_help() -{ - echo "scriptname usage:" - echo $usage_string - echo -e "\t-a astrobee_source_path specify the astrobee source directory to use" - echo -e "\t default=isaac_source/../../astrobee" - echo -e "\t-d idi_source_path specify the idi source directory to use" - echo -e "\t default=isaac_source/../../isaac_data_interface" - echo -e "\t-m mast_source_path specify the mast source directory to use" - echo -e "\t default=isaac_source/../../mast/src" - echo -e "\t-n build without MAST" - echo -} +# Initialize variables os=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` mast=1 vm=0 @@ -60,16 +62,16 @@ while [ "$1" != "" ]; do ;; -f | --focal ) os="focal" ;; - -a | --astrobee_source_dir ) shift + -a | --astrobee-source-path ) shift astrobee_source=$1 ;; - -d | --idi_source_dir ) shift + -i | --iui-source-dir ) shift idi_source=$1 ;; - -m | --mast_source_dir ) shift + -m | --mast-source-dir ) shift mast_source=$1 ;; - -n | --no_mast ) mast=0 + -n | --no-mast ) mast=0 ;; -v | --vm ) vm=1 ;; diff --git a/scripts/docker/docker_compose/analyst.docker-compose.yml b/scripts/docker/docker_compose/analyst.docker-compose.yml new file mode 100644 index 00000000..702e3993 --- /dev/null +++ b/scripts/docker/docker_compose/analyst.docker-compose.yml @@ -0,0 +1,27 @@ +version: "2.1" +services: + + + analyst_notebook: + # Launches analyst notebook + # + image: isaac_analyst_notebook + hostname: isaac_notebook + container_name: isaac_notebook + build: + context: '..' + dockerfile: 'analyst.Dockerfile' + args: + REMOTE: ${REMOTE:-isaac} + ports: + - "8888:8888" + networks: + - isaac + environment: + - JUPYTER_TOKEN=isaac + - ROS_MASTER_URI=http://rosmaster:11311 + volumes: + - ${ISAAC_PATH:-.}/analyst/workspace:/home/analyst/ + - ${DATA_PATH}:/home/analyst/data + - ${BAGS_PATH}:/home/analyst/data/bags + command: "/ros_entrypoint.sh" diff --git a/scripts/docker/docker_compose/idi.docker-compose.yml b/scripts/docker/docker_compose/idi.docker-compose.yml index df811ddc..c2ede22e 100644 --- a/scripts/docker/docker_compose/idi.docker-compose.yml +++ b/scripts/docker/docker_compose/idi.docker-compose.yml @@ -42,18 +42,17 @@ services: # -------------------------------------------------------------------------------------------------- # Essential IDI Services # -------------------------------------------------------------------------------------------------- - idi_frontend: + iui_frontend: # This service serves the static content that represents # the frontend ISAAC data interface. # - image: isaac_data_interface_frontend - build: "${IDI_PATH:-.}/frontend" - hostname: idi_frontend - container_name: idi_frontend + image: ${REMOTE}/isaac_user_interface_frontend + hostname: iui_frontend + container_name: iui_frontend ports: - "8080:8080" depends_on: - - idi_backend + - iui_backend - rosbridge networks: - isaac @@ -62,35 +61,34 @@ services: # volumes: # - "./frontend:/app:ro" - idi_backend: + iui_backend: # This service serves the front-end by providing it with # historical telemetry that it retrieves from ROS bridge. # - image: isaac_data_interface_backend - build: "${IDI_PATH:-.}/backend" - hostname: idi_backend - container_name: idi_backend + image: ${REMOTE}/isaac_user_interface_backend + hostname: iui_backend + container_name: iui_backend command: python /main.py volumes: - - "${IDI_PATH:-.}/config.json:/config.json:ro" + - "${IUI_PATH:-.}/config.json:/config.json:ro" depends_on: - rosmaster - rosbridge - - idi_arangodb + - iui_arangodb ports: - "9091:9091" networks: - isaac links: - - idi_arangodb + - iui_arangodb - idi_arangodb: + iui_arangodb: # This service provides a database for the backend. The # backend uses this database to store ROS messages in JSON format. # image: arangodb:latest - hostname: idi_arangodb - container_name: idi_arangodb + hostname: iui_arangodb + container_name: iui_arangodb environment: ARANGO_ROOT_PASSWORD: isaac ports: @@ -108,13 +106,7 @@ services: # For more info, see: # http://wiki.ros.org/rosbridge_suite # - image: isaac_data_interface_rosbridge - build: - context: "${IDI_PATH:-.}/rosbridge" - args: - UBUNTU_VERSION: '${UBUNTU_VERSION}' - ROS_VERSION: '${ROS_VERSION}' - PYTHON: '${PYTHON}' + image: ${REMOTE}/isaac_user_interface_rosbridge hostname: rosbridge container_name: rosbridge entrypoint: "/ros_entrypoint.sh" @@ -130,87 +122,6 @@ services: isaac: ipv4_address: 172.19.0.8 - rosvideo: - # This service runs a web video server that will efficiently - # publish a stream of images from a ROS topic that is set to - # publish ROS messages of the type sensor_msgs/Image - # - # For more info, see: - # http://wiki.ros.org/web_video_server - # - image: isaac_data_interface_rosvideo - build: - context: "${IDI_PATH:-.}/rosvideo" - args: - UBUNTU_VERSION: '${UBUNTU_VERSION}' - ROS_VERSION: '${ROS_VERSION}' - PYTHON: '${PYTHON}' - hostname: rosvideo - container_name: rosvideo - entrypoint: "/ros_entrypoint.sh" - command: "rosrun web_video_server web_video_server" - expose: - - 8080 - depends_on: - - rosmaster - - rosbridge - environment: - - ROS_MASTER_URI=http://rosmaster:11311 - - ROS_IP=172.19.0.9 - networks: - isaac: - ipv4_address: 172.19.0.9 - - idi_transformers: - # This service is the master of meshes: it subscribes to these - # three ROS topics: - # 1. /ism/sci_cam/img - # 2. /ism/sci_cam/obj - # 3. /ism/sci_cam/mtl - # When it recieves a complete mesh from all three topics, it - # assembles them and creates a GLTF file. Then it publishes on - # the following ROS topic to let the IDI frontend know that - # a new mesh is available. - # 1. /mapper_gltf - # - image: isaac_data_interface_transformers - build: - context: "${IDI_PATH:-.}/transformers" - args: - UBUNTU_VERSION: '${UBUNTU_VERSION}' - ROS_VERSION: '${ROS_VERSION}' - PYTHON: '${PYTHON}' - hostname: idi_transformers - container_name: idi_transformers - command: /transformers/run.sh - depends_on: - - rosmaster - environment: - - ROS_MASTER_URI=http://rosmaster:11311 - - ROS_IP=172.19.0.7 - networks: - isaac: - ipv4_address: 172.19.0.7 - - idi_notebook: - # Launches analyst notebook - # - image: isaac_data_interface_notebook - build: "${IDI_PATH:-.}/notebook" - hostname: idi_notebook - container_name: idi_notebook - ports: - - "8888:8888" - environment: - - JUPYTER_TOKEN=isaac - - ROS_MASTER_URI=http://rosmaster:11311 - depends_on: - - rosmaster - volumes: - - ${IDI_PATH:-.}/notebook/workspace:/home/jovyan/workspace/ - networks: - - isaac - command: "start-notebook.sh" # ---------------------------------------------------------------------------------------------------- # Docker Compose Volumes diff --git a/scripts/docker/readme.md b/scripts/docker/readme.md index 6a20a941..57223929 100644 --- a/scripts/docker/readme.md +++ b/scripts/docker/readme.md @@ -1,6 +1,74 @@ -\page docker Docker +\page docker Docker Install -## Demos +Install dependencies +--------- + +Install docker tools: https://docs.docker.com/engine/install/ubuntu/ + +Install docker compose: https://docs.docker.com/compose/install/ + +Install nvidia-docker: https://github.com/NVIDIA/nvidia-docker. + + +Building the docker images +--------- + +To run the demos, you can use the remote images and skip this section. +If you want to build the docker images locally instead of pulling from the remote repository, run: + + ./build.sh [OPTIONS] +The build script will automatically detect the current Ubuntu OS version and define the docker files variables +`UBUNTU_VERSION`, `ROS_VERSION`, and `PYTHON` accordingly. + + +Options: + +If a specific version is desired, the option --xenial, --bionic, +and --focal is used for ubuntu 16.04, 18.04, and 20.04 docker images, respectively. + +If you are building the docker images in a virtual machine, because of graphics card passthrough +restriction, the gazebo simulation will not work properly. Therefore the build script has the option --vm +which will only build the images that are used for a virtual machine deployment (simulation runs natively). + +If you don't want to run mast or don't have access to it (not a public repository), the use the option --no-mast. + +For further help on available options: + + ./build --help + +Run the docker containers +--------- + +To run the docker containers: + + ./run.sh [OPTIONS] + +It will automatically detect the current Ubuntu OS version. If a specific version is desired, the option +--xenial, --bionic, and --focal is used for ubuntu 16.04, 18.04, and 20.04 docker images, respectively. + +Options: + +If a specific version is desired, the option --xenial, --bionic, +and --focal is used for ubuntu 16.04, 18.04, and 20.04 docker images, respectively. +Note this is only valid for the docker containers, native software will run on the host OS. + + + + + + +For further help on available options: + + ./run --help + + + + + +http://localhost:8888/lab?token=isaac + +Docker Demos +---------- There are currently 3 demos available to showcase some aspects of the ISAAC functionality @@ -16,4 +84,10 @@ The robot will undock, follow a trajectory taking pictures at the specified wayp ### Trigger wifi mapping This demo will trigger a volumetric mapping inspection event. The volumetric mapper collects information from an onboard sensor of Astrobee and interpolates the data in a specified area. -The robot will undock, follow a trajectory and dock again. For the wifi mapper, the trajectory followed is defined in astrobee/behaviors/inspection/resources/wifi.txt. \ No newline at end of file +The robot will undock, follow a trajectory and dock again. For the wifi mapper, the trajectory followed is defined in astrobee/behaviors/inspection/resources/wifi.txt. + + +Open `http://127.0.0.1:8080` in a web browser to see what is happening. Use +`docker ps` to see the docker containers and use `docker exec -it container_name /bin/bash` to get a shell in one. + +Cancel with Ctrl+c and then run `scripts/docker/shutdown.sh` to stop the demo. diff --git a/scripts/docker/run.sh b/scripts/docker/run.sh index 917997da..4bd8bfb1 100755 --- a/scripts/docker/run.sh +++ b/scripts/docker/run.sh @@ -18,29 +18,180 @@ # License for the specific language governing permissions and limitations # under the License. +set -e + +# Print the help message (list all the options) +print_help() +{ + echo -e "Builds docker images for the ISAAC stack" + echo -e "The build script will automatically detect the current Ubuntu OS version and" + echo -e "define the docker files variables UBUNTU_VERSION, ROS_VERSION, and PYTHON accordingly." + echo -e "Options:" + echo -e "\t-x | --xenial\t\t\tRun images for Ubuntu 16.04" + echo -e "\t-b | --bionic\t\t\tRun images for Ubuntu 18.04" + echo -e "\t-f | --focal\t\t\tRun images for Ubuntu 20.04" + echo -e "\t-i | --iui-source-dir\t\tSpecify the idi source directory to use" + echo -e "\t\t\t\tdefault=isaac_source/../../isaac_data_interface" + echo -e "\t-m | --mast-source-dir\t\tSpecify the mast source directory to use" + echo -e "\t\t\t\tdefault=isaac_source/../../mast/src" + echo -e "\t--no-mast\t\t\tDon't run MAST" + echo -e "\t-vm\t\t\t\tRun the simulation locally - compatible with VM setup" + echo -e "\t-remote\t\t\t\tRun the remote images (bypass building docker images locally)" + echo -e "\t-analyst\t\t\tRun analyst notebook" + echo -e "\t--no-simvm\t\t\tDon't run the simulation" + echo -e "\t-g | --ground-only\t\tRun only the isaac ground software (hardware in the loop only)" + echo -e "\t-robot\t\t\t\tSpecify robot name (hardware in the loop only)" + echo +} + +# Print the script usage +print_usage() +{ + echo -e "Invalid option!\n\n" + print_help +} + +# Initialize variables +os=`cat /etc/os-release | grep -oP "(?<=VERSION_CODENAME=).*"` +mast=1 # MAST is ON by default +vm=0 # We are not running in a virtual machine by default +remote=0 +ground=0 +analyst= +robot=bumble +no_sim=0 + +while [ "$1" != "" ]; do + case $1 in + -x | --xenial ) os="xenial" + ;; + -b | --bionic ) os="bionic" + ;; + -f | --focal ) os="focal" + ;; + -d | --iui-source-dir ) shift + iui_source=$1 + ;; + -m | --mast-source-dir ) shift + mast_source=$1 + ;; + --no-mast ) mast=0 + ;; + -vm ) vm=1 + ;; + -remote ) remote=1 + ;; + -analyst ) analyst=1 + ;; + --no-sim ) no_sim=1 + ;; + -g | --ground-only ) ground=1 + ;; + -robot ) shift + robot=$1 + ;; + -h | --help ) print_help + exit + ;; + * ) print_usage + exit 1 + esac + shift +done + thisdir=$(dirname "$(readlink -f "$0")") -rootdir=${thisdir}/../../.. -export IDI_PATH=${IDI_PATH:-${rootdir}/isaac_data_interface} -export MAST_PATH=${MAST_PATH:-${rootdir}/mast} -cd $thisdir -# docker-compose -f docker-compose.yml -f ${IDI_PATH}/idi.docker-compose.yml -f mast.yml up -d && docker-compose ps -docker-compose -f ${IDI_PATH}/ros.docker-compose.yml -f ${IDI_PATH}/idi.docker-compose.yml up -d && docker-compose ps - -XSOCK=/tmp/.X11-unix -XAUTH=/tmp/.docker.xauth -touch $XAUTH -xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - - -docker run -d --rm --name isaac \ - --network docker_isaac \ - --volume=$XSOCK:$XSOCK:rw \ - --volume=$XAUTH:$XAUTH:rw \ - --volume=`pwd`/simulation.config:/src/astrobee/astrobee/config/simulation/simulation.config:ro \ - --volume=`pwd`/simulation.config:/opt/astrobee/config/simulation/simulation.config:ro \ - --env="XAUTHORITY=${XAUTH}" \ - --env="DISPLAY" \ - --user="astrobee" \ - --env="ROS_MASTER_URI=http://rosmaster:11311" \ - --gpus all \ - isaac \ - /ros_entrypoint.sh roslaunch isaac sim.launch dds:=false wifi:=station streaming_mapper:=true acoustics_cam:=true +rootdir=${thisdir}/../.. + +# Define root dir of different repos +isaac_source=${rootdir}/./ +iui_source=${iui_source:-${rootdir}/../../isaac_user_interface} +mast_source=${mast_source:-${rootdir}/../../mast/src} +robot=${robot:-bumble} + +export ISAAC_PATH=${isaac_source} +export IUI_PATH=${iui_source} +export MAST_PATH=${mast_source} + +# Define data locations for analyst notebook +export DATA_PATH=${HOME}/data +export BAGS_PATH=$(readlink -f ${HOME}/data/bags) + +echo "ISAAC UI path: "${iui_source} +echo "Build MAST?:" $mast " MAST path: "${mast_source} + +UBUNTU_VERSION=16.04 +ROS_VERSION=kinetic +PYTHON='' + +if [ "$os" = "bionic" ]; then + UBUNTU_VERSION=18.04 + ROS_VERSION=melodic + PYTHON='' + +elif [ "$os" = "focal" ]; then + UBUNTU_VERSION=20.04 + ROS_VERSION=noetic + PYTHON='3' +fi + +if [ $remote -eq 1 ]; then + export REMOTE=ghcr.io/nasa +fi + +# Launch the rosmaster container + isaac network + IDI +files="-f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml" +echo -e "The ISAAC UI is hosted in: http://localhost:8080" +echo -e "The ArangoDB database is hosted in: http://localhost:8529" + +# Launch MAST +if [ $mast -eq 1 ]; then + files+=" -f ${thisdir}/docker_compose/mast.docker_compose.yml" +fi + +# Launch the analyst notebook +if [ $analyst -eq 1 ]; then + files+=" -f ${thisdir}/docker_compose/analyst.docker-compose.yml" + echo -e "The Analyst Notebook is hosted in: http://localhost:8888/lab?token=isaac" +fi + +docker-compose ${files} up -d + +if [ $ground -eq 1 ]; then + echo "GROUND" + # Start native astrobee simulation and connect to socket network + export ROS_IP=`ip -4 addr show docker0 | grep -oP "(?<=inet ).*(?=/)"` + export ROS_MASTER_URI=http://172.19.0.5:11311 + roslaunch isaac isaac_astrobee.launch llp:=disabled mlp:=disabled ilp:=disabled streaming_mapper:=true output:=screen robot:=${robot} --wait + +elif [ $vm -eq 1 ]; then + echo "VM" + # Start native astrobee simulation and connect to socket network + export ROS_IP=`ip -4 addr show docker0 | grep -oP "(?<=inet ).*(?=/)"` + export ROS_MASTER_URI=http://172.19.0.5:11311 + roslaunch isaac isaac_astrobee.launch llp:=disabled mlp:=disabled ilp:=disabled streaming_mapper:=true output:=screen robot:=${robot} --wait + +elif [ $no_sim -eq 0 ]; then + echo "NO SIM" + XSOCK=/tmp/.X11-unix + XAUTH=/tmp/.docker.xauth + touch $XAUTH + xauth nlist $DISPLAY | sed -e 's/^..../ffff/' | xauth -f $XAUTH nmerge - + + docker run -d --rm --name isaac \ + --network docker_isaac \ + --volume=$XSOCK:$XSOCK:rw \ + --volume=$XAUTH:$XAUTH:rw \ + --volume=`pwd`/simulation.config:/src/astrobee/astrobee/config/simulation/simulation.config:ro \ + --volume=`pwd`/simulation.config:/opt/astrobee/config/simulation/simulation.config:ro \ + --env="XAUTHORITY=${XAUTH}" \ + --env="DISPLAY" \ + --user="astrobee" \ + --env="ROS_MASTER_URI=http://rosmaster:11311" \ + --gpus all \ + isaac \ + /ros_entrypoint.sh roslaunch isaac sim.launch dds:=false wifi:=station streaming_mapper:=true acoustics_cam:=true + +fi + + + diff --git a/scripts/docker/run_ground.sh b/scripts/docker/run_ground.sh deleted file mode 100755 index 4049cd84..00000000 --- a/scripts/docker/run_ground.sh +++ /dev/null @@ -1,84 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2021, 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. - -set -e - -# short help -usage_string="$scriptname [-h] [-a ] [-i ] [-d ]" -#[-t make_target] - -usage() -{ - echo "usage: sysinfo_page [[[-a] [-i] [-d] [-m] [-n] [-r]] | [-h]]" -} -mast=1 - -while [ "$1" != "" ]; do - case $1 in - -a | --astrobee_source_dir ) shift - astrobee_source=$1 - ;; - -d | --idi_source_dir ) shift - idi_source=$1 - ;; - -m | --mast_source_dir ) shift - mast_source=$1 - ;; - -n | --no_mast ) mast=0 - ;; - -r | --robot ) shift - robot=$1 - ;; - -h | --help ) usage - exit - ;; - * ) usage - exit 1 - esac - shift -done - -# Find out isaac root dir -thisdir=$(dirname "$(readlink -f "$0")") -rootdir=${thisdir}/../.. - -# Define root dir of different repos -isaac_source=${rootdir}/./ -idi_source=${idi_source:-${rootdir}/../../isaac_data_interface} -mast_source=${mast_source:-${rootdir}/../../mast/src} -robot=${robot:-bumble} - -export IDI_PATH=${idi_source} -export MAST_PATH=${mast_source} - -echo "IDI path: "${idi_source} -echo "Build MAST?:" $mast " MAST path: "${mast_source} - -# Launch the rosmaster container + isaac network + IDI -if [ $mast == 1 ]; then - docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml -f ${thisdir}/docker_compose/mast.docker_compose.yml up -d -else - docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml up -d -fi - -# Start native astrobee simulation and connect to socket network -export ROS_IP=`ip -4 addr show docker0 | grep -oP "(?<=inet ).*(?=/)"` -export ROS_MASTER_URI=http://172.19.0.5:11311 -roslaunch isaac isaac_astrobee.launch llp:=disabled mlp:=disabled ilp:=disabled streaming_mapper:=true output:=screen robot:=${robot} --wait diff --git a/scripts/docker/run_vm.sh b/scripts/docker/run_vm.sh deleted file mode 100755 index eec0ade4..00000000 --- a/scripts/docker/run_vm.sh +++ /dev/null @@ -1,80 +0,0 @@ -#!/bin/bash -# -# Copyright (c) 2021, 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. - -set -e - -# short help -usage_string="$scriptname [-h] [-a ] [-i ] [-d ]" -#[-t make_target] - -usage() -{ - echo "usage: sysinfo_page [[[-a] [-i] [-d] [-m] [-n]] | [-h]]" -} -mast=1 - -while [ "$1" != "" ]; do - case $1 in - -a | --astrobee_source_dir ) shift - astrobee_source=$1 - ;; - -d | --idi_source_dir ) shift - idi_source=$1 - ;; - -m | --mast_source_dir ) shift - mast_source=$1 - ;; - -n | --no_mast ) mast=0 - ;; - -h | --help ) usage - exit - ;; - * ) usage - exit 1 - esac - shift -done - -# Find out isaac root dir -thisdir=$(dirname "$(readlink -f "$0")") -rootdir=${thisdir}/../.. - -# Define root dir of different repos -isaac_source=${rootdir}/./ -idi_source=${idi_source:-${rootdir}/../../isaac_data_interface} -mast_source=${mast_source:-${rootdir}/../../mast/src} - -export IDI_PATH=${idi_source} -export MAST_PATH=${mast_source} - -echo "IDI path: "${idi_source} -echo "Build MAST?:" $mast " MAST path: "${mast_source} - -# Launch the rosmaster container + isaac network + IDI -if [ $mast == 1 ]; then - docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml -f ${thisdir}/docker_compose/mast.docker_compose.yml up -d -else - docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml up -d -fi - -# Start native astrobee simulation and connect to socket network -export ROS_IP=`ip -4 addr show docker0 | grep -oP "(?<=inet ).*(?=/)"` -export ROS_MASTER_URI=http://172.19.0.5:11311 -roslaunch isaac sim.launch rviz:=true dds:=false robot:=sim_pub ns:=queen streaming_mapper:=true --wait diff --git a/scripts/docker/shutdown.sh b/scripts/docker/shutdown.sh index 84324a33..81f32c69 100755 --- a/scripts/docker/shutdown.sh +++ b/scripts/docker/shutdown.sh @@ -19,10 +19,12 @@ # under the License. thisdir=$(dirname "$(readlink -f "$0")") -rootdir=${thisdir}/../../.. -export IDI_PATH=${IDI_PATH:-${rootdir}/../isaac_data_interface} -export MAST_PATH=${MAST_PATH:-${rootdir}/../mast} -docker stop isaac -docker rm isaac -docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml -f ${thisdir}/docker_compose/mast.docker_compose.yml down \ + +# stop isaac container if it exists +docker stop isaac 2>/dev/null +docker rm isaac 2>/dev/null + +# stop all docker-compose combinations +docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml -f ${thisdir}/docker_compose/mast.docker_compose.yml -f ${thisdir}/docker_compose/analyst.docker-compose.yml down \ +||docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml -f ${thisdir}/docker_compose/mast.docker_compose.yml down \ || docker-compose -f ${thisdir}/docker_compose/ros.docker-compose.yml -f ${thisdir}/docker_compose/idi.docker-compose.yml down From bea0870ab458943e8aa5f79a30e7801a98751041 Mon Sep 17 00:00:00 2001 From: kbrowne15 Date: Fri, 4 Mar 2022 17:24:17 -0800 Subject: [PATCH 2/6] Added sci cam gazebo plugin. Removed from astrobee (#41) * Added sci cam gazebo plugin. Removed from astrobee * Json header not found fix. --- .../description/urdf/isaac_model.urdf.xacro | 2 +- .../urdf/sensor_sci_cam.urdf.xacro | 46 +++ .../simulation/isaac_gazebo/CMakeLists.txt | 16 + astrobee/simulation/isaac_gazebo/package.xml | 4 + .../gazebo_sensor_plugin_sci_cam.cc | 358 ++++++++++++++++++ isaac/config/simulation/simulation.config | 35 ++ 6 files changed, 460 insertions(+), 1 deletion(-) create mode 100644 astrobee/description/description/urdf/sensor_sci_cam.urdf.xacro create mode 100644 astrobee/simulation/isaac_gazebo/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc create mode 100644 isaac/config/simulation/simulation.config diff --git a/astrobee/description/description/urdf/isaac_model.urdf.xacro b/astrobee/description/description/urdf/isaac_model.urdf.xacro index 29a53d42..7b05559d 100644 --- a/astrobee/description/description/urdf/isaac_model.urdf.xacro +++ b/astrobee/description/description/urdf/isaac_model.urdf.xacro @@ -134,7 +134,7 @@ - + diff --git a/astrobee/description/description/urdf/sensor_sci_cam.urdf.xacro b/astrobee/description/description/urdf/sensor_sci_cam.urdf.xacro new file mode 100644 index 00000000..9104e038 --- /dev/null +++ b/astrobee/description/description/urdf/sensor_sci_cam.urdf.xacro @@ -0,0 +1,46 @@ + + + + + + + + + + + + + + + + + + + + + + + 0 0 0 0 0 0 + true + 0.0001 + + 1.0821 + + 5344 + 4008 + B8G8R8 + + + 0.1 + 20 + + + 0.0 + + + + /${ns}/ + + + + diff --git a/astrobee/simulation/isaac_gazebo/CMakeLists.txt b/astrobee/simulation/isaac_gazebo/CMakeLists.txt index e705bd0d..edd87bf2 100644 --- a/astrobee/simulation/isaac_gazebo/CMakeLists.txt +++ b/astrobee/simulation/isaac_gazebo/CMakeLists.txt @@ -34,6 +34,8 @@ find_package(catkin REQUIRED COMPONENTS isaac_hw_msgs isaac_msgs cv_bridge + camera + jsonloader ) find_package(gazebo REQUIRED) @@ -45,6 +47,7 @@ catkin_package( gazebo_sensor_plugin_wifi_receiver gazebo_sensor_plugin_wifi_transmitter gazebo_sensor_plugin_heat_cam + gazebo_sensor_plugin_sci_cam gazebo_model_plugin_cargo CATKIN_DEPENDS gazebo_plugins @@ -58,6 +61,8 @@ catkin_package( isaac_util isaac_msgs isaac_hw_msgs + camera + jsonloader ) ########### @@ -154,6 +159,17 @@ add_dependencies( ${catkin_EXPORTED_TARGETS}) list(APPEND targets_to_install gazebo_sensor_plugin_air_quality) +# Create a sensor plugin for the sci cam +add_library( + gazebo_sensor_plugin_sci_cam + SHARED src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc) +target_link_libraries( + gazebo_sensor_plugin_sci_cam + ${catkin_LIBRARIES} ${GAZEBO_LIBRARIES}) +add_dependencies( + gazebo_sensor_plugin_sci_cam + ${catkin_EXPORTED_TARGETS}) +list(APPEND targets_to_install gazebo_sensor_plugin_sci_cam) # Create a model plugin for the cargo system add_library( diff --git a/astrobee/simulation/isaac_gazebo/package.xml b/astrobee/simulation/isaac_gazebo/package.xml index 23abc092..a3177e3b 100644 --- a/astrobee/simulation/isaac_gazebo/package.xml +++ b/astrobee/simulation/isaac_gazebo/package.xml @@ -26,6 +26,8 @@ isaac_util isaac_hw_msgs isaac_msgs + camera + jsonloader roscpp gazebo_ros gazebo_plugins @@ -37,6 +39,8 @@ isaac_util isaac_hw_msgs isaac_msgs + camera + jsonloader diff --git a/astrobee/simulation/isaac_gazebo/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc b/astrobee/simulation/isaac_gazebo/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc new file mode 100644 index 00000000..055480cd --- /dev/null +++ b/astrobee/simulation/isaac_gazebo/src/gazebo_sensor_plugin_sci_cam/gazebo_sensor_plugin_sci_cam.cc @@ -0,0 +1,358 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform 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 + * + * https://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. + */ + +// ROS includes +#include + +// FSW includes +#include + +// JSON includes +#include +#include +#include + +// Sensor plugin interface +#include + +// Sensor and fsw messages +#include +#include +#include +#include +#include +#include +#include +#include + +// Open CV Includes +#include +#include +#include + +// STL includes +#include +#include +#include + +namespace gazebo { + +class GazeboSensorPluginSciCam : public FreeFlyerSensorPlugin { + public: + GazeboSensorPluginSciCam() : + FreeFlyerSensorPlugin("sci_cam", "sci_cam", false), + continuousPictureTaking_(false), takeSinglePicture_(false), rate_(0.0) {} + + ~GazeboSensorPluginSciCam() { + if (update_) { + #if GAZEBO_MAJOR_VERSION > 7 + update_.reset(); + #else + sensor_->DisconnectUpdated(update_); + #endif + } + } + + protected: + // Called when plugin is loaded into gazebo + void LoadCallback(ros::NodeHandle *nh, + sensors::SensorPtr sensor, sdf::ElementPtr sdf) { + // Get a link to the parent sensor + sensor_ = std::dynamic_pointer_cast(sensor); + if (!sensor_) { + gzerr << "GazeboSensorPluginSciCam requires a parent camera sensor.\n"; + return; + } + + // Check that we have a color camera + if (sensor_->Camera()->ImageFormat() != "B8G8R8") + ROS_FATAL_STREAM("Camera format must be B8G8R8"); + + // Set image constants + sci_cam_image_msg_.header.frame_id = GetFrame(); + sci_cam_image_msg_.encoding = sensor_msgs::image_encodings::BGR8; + sci_cam_image_msg_.header.frame_id = "camera"; + sci_cam_image_msg_compressed_.format = "jpeg"; + + // Compression settings + compression_params_.resize(9, 0); + compression_params_[0] = cv::IMWRITE_JPEG_QUALITY; + compression_params_[1] = 80; // JPEG quality percentile (1-100) + compression_params_[2] = cv::IMWRITE_JPEG_PROGRESSIVE; + compression_params_[3] = 0; // Enable compression to progressive JPEG + compression_params_[4] = cv::IMWRITE_JPEG_OPTIMIZE; + compression_params_[5] = 0; // Enable JPEG compress optimization + compression_params_[6] = cv::IMWRITE_JPEG_RST_INTERVAL; + compression_params_[7] = 0; // JPEG restart interval (0-65535) + + // Create subscriber to commands though which the sci cam will be controlled + dds_cmd_sub_ = nh->subscribe(TOPIC_COMMAND, 10, + &GazeboSensorPluginSciCam::CmdCallback, this); + // Acknowledge publisher to confirm the message was received + pub_sci_cam_ack_ = nh->advertise(TOPIC_GUEST_SCIENCE_MANAGER_ACK, 10); + + // Create publishers for sci cam image, pose, and camera info + std::string compressed_topic = TOPIC_HARDWARE_SCI_CAM; + compressed_topic += "/compressed"; + pub_sci_cam_image_ = nh->advertise(compressed_topic, 2, + boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this), + boost::bind(&GazeboSensorPluginSciCam::ToggleCallback, this)); + pub_sci_cam_pose_ = nh->advertise(TOPIC_SCI_CAM_SIM_POSE, 10); + pub_sci_cam_info_ = nh->advertise(TOPIC_SCI_CAM_SIM_INFO, 10); + + // Read configuration + config_reader::ConfigReader config; + + // Set the config path to ISAAC + char *path; + path = getenv("ISAAC_CONFIG_DIR"); + if (path == NULL) { + ROS_FATAL("Could not find the config path."); + return; + } + + config.SetPath(path); + + config.AddFile("simulation/simulation.config"); + if (!config.ReadFiles()) { + ROS_FATAL("Failed to read simulation config file."); + return; + } + bool dos = true; + if (!config.GetBool("disable_cameras_on_speedup", &dos)) + ROS_FATAL("Could not read the disable_cameras_on_speedup parameter."); + if (!config.GetReal("sci_cam_rate", &rate_)) + ROS_FATAL("Could not read the sci_cam_rate parameter."); + + bool cp; + if (config.GetBool("sci_cam_continuous_picture_taking", &cp)) { + continuousPictureTaking_ = cp; + } + config.Close(); + + // If we have a sped up simulation and we need to disable the camera + double simulation_speed = 1.0; + if (nh->getParam("/simulation_speed", simulation_speed)) + if (simulation_speed > 1.0 && dos) rate_ = 0.0; + + // Toggle if the camera is active or not + ToggleCallback(); + } + + // Only send measurements when extrinsics are available + void OnExtrinsicsReceived(ros::NodeHandle *nh) { + // Connect to the camera update event. + update_ = sensor_->ConnectUpdated( + std::bind(&GazeboSensorPluginSciCam::ImageCallback, this)); + } + + // Turn camera on or off based on topic subscription + void ToggleCallback() { + if (pub_sci_cam_image_.getNumSubscribers() > 0 && rate_ > 0) { + sensor_->SetUpdateRate(rate_); + sensor_->SetActive(true); + } else { + sensor_->SetUpdateRate(0.0001); + sensor_->SetActive(false); + } + } + + // Called when a dds command is received. Process only guest science + // sci cam control commands. + void CmdCallback(ff_msgs::CommandStamped const& cmd) { + // Process only guest science commands + if (cmd.cmd_name != ff_msgs::CommandConstants::CMD_NAME_CUSTOM_GUEST_SCIENCE) { + // Only process custom sci cam commands + return; + } + + if (cmd.args.size() != 2) { + // Custom sci cam commands have two arguments + return; + } + + if (cmd.args[0].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING || + cmd.args[1].data_type != ff_msgs::CommandArg::DATA_TYPE_STRING ) { + return; + } + + std::string app_name = cmd.args[0].s.data(); + + // Process only sci cam commands + if (app_name != "gov.nasa.arc.irg.astrobee.sci_cam_image") { + return; + } + + std::string json_str = cmd.args[1].s.data(); + ROS_INFO_STREAM("Received command: " << json_str); + + // Convert string into a json object + Json::Value cmd_obj; + if (!jsonloader::LoadData(json_str, &cmd_obj)) { + ROS_ERROR_STREAM("Unable to convert command " << json_str << " to json."); + return; + } + + // Check to make sure command name exists + if (!cmd_obj.isMember("name") || !cmd_obj["name"].isString()) { + ROS_ERROR_STREAM(json_str << " doesn't contain name for the command."); + return; + } + + std::string action = cmd_obj["name"].asString(); + + // Record the desired intention. Use a lock. + { + const std::lock_guard lock(sci_cam_image_lock); + if (action == "takePicture") { + takeSinglePicture_ = true; + continuousPictureTaking_ = false; + } else if (action == "setContinuousPictureTaking") { + if (cmd_obj.isMember("continuous") && cmd_obj["continuous"].isBool()) { + if (cmd_obj["continuous"].asBool()) { + takeSinglePicture_ = false; + continuousPictureTaking_ = true; + } else { + takeSinglePicture_ = false; + continuousPictureTaking_ = false; + } + } else { + ROS_ERROR_STREAM("Got set continuous picture taking command but it" << + " didn't contain the continuous argument."); + } + } else { + ROS_FATAL_STREAM("Unknown sci_cam command: " << action); + } + } + + ff_msgs::AckStamped msg_ack; + msg_ack.header.stamp = ros::Time::now(); + msg_ack.cmd_id = cmd.cmd_id; + msg_ack.status.status = ff_msgs::AckStatus::COMPLETED; + msg_ack.completed_status.status = ff_msgs::AckCompletedStatus::OK; + pub_sci_cam_ack_.publish(msg_ack); + + return; + } + + // Called when a new image must be rendered + void ImageCallback() { + // Quickly record the current time and current pose before doing other computations + ros::Time curr_time = ros::Time::now(); + + // Publish the sci cam pose + #if GAZEBO_MAJOR_VERSION > 7 + Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->WorldPose(), sensor_->Pose()); + #else + Eigen::Affine3d sensor_to_world = SensorToWorld(GetModel()->GetWorldPose(), sensor_->Pose()); + #endif + sci_cam_pose_msg_.header.frame_id = GetFrame(); + + sci_cam_pose_msg_.header.stamp = curr_time; // it is very important to get the time right + sci_cam_pose_msg_.pose.position.x = sensor_to_world.translation().x(); + sci_cam_pose_msg_.pose.position.y = sensor_to_world.translation().y(); + sci_cam_pose_msg_.pose.position.z = sensor_to_world.translation().z(); + Eigen::Quaterniond q(sensor_to_world.rotation()); + sci_cam_pose_msg_.pose.orientation.w = q.w(); + sci_cam_pose_msg_.pose.orientation.x = q.x(); + sci_cam_pose_msg_.pose.orientation.y = q.y(); + sci_cam_pose_msg_.pose.orientation.z = q.z(); + pub_sci_cam_pose_.publish(sci_cam_pose_msg_); + + // Publish the sci cam intrinsics + sci_cam_info_msg_.header.frame_id = GetFrame(); + sci_cam_info_msg_.header.stamp = curr_time; // it is very important to get the time right + FillCameraInfo(sensor_->Camera(), sci_cam_info_msg_); // fill in from the camera pointer + pub_sci_cam_info_.publish(sci_cam_info_msg_); + + // Do not publish unless specifically told to + if (!continuousPictureTaking_ && !takeSinglePicture_) { + return; + } + + // Make sci cam image topic + // Record not the current time, but the time when the image was acquired + sci_cam_image_msg_.header.stamp.sec = sensor_->LastMeasurementTime().sec; + sci_cam_image_msg_.header.stamp.nsec = sensor_->LastMeasurementTime().nsec; + sci_cam_image_msg_.height = sensor_->ImageHeight(); + sci_cam_image_msg_.width = sensor_->ImageWidth(); + sci_cam_image_msg_.step = sci_cam_image_msg_.width * 3; + sci_cam_image_msg_.data.resize(sci_cam_image_msg_.step * sci_cam_image_msg_.height); + const uint8_t* data_start = reinterpret_cast(sensor_->ImageData()); + std::copy(data_start, data_start + sci_cam_image_msg_.step * sci_cam_image_msg_.height, + sci_cam_image_msg_.data.begin()); + + // Make sci cam image compressed topic + // Record not the current time, but the time when the image was acquired + sci_cam_image_msg_compressed_.header.stamp.sec = sensor_->LastMeasurementTime().sec; + sci_cam_image_msg_compressed_.header.stamp.nsec = sensor_->LastMeasurementTime().nsec; + + // OpenCV-ros bridge + try { + cv_bridge::CvImagePtr cv_ptr = cv_bridge::toCvCopy(sci_cam_image_msg_, sensor_msgs::image_encodings::BGR8); + + // Compress image + if (cv::imencode(".jpg", cv_ptr->image, sci_cam_image_msg_compressed_.data, compression_params_)) { + // Publish the compressed message + pub_sci_cam_image_.publish(sci_cam_image_msg_compressed_); + // Done taking a single picture. Use a lock to change this flag. + if (takeSinglePicture_) { + const std::lock_guard lock(sci_cam_image_lock); + takeSinglePicture_ = false; + } + } else { + ROS_ERROR("cv::imencode (jpg) failed on input image"); + } + } + catch (cv_bridge::Exception& e) { + ROS_ERROR("%s", e.what()); + return; + } + catch (cv::Exception& e) { + ROS_ERROR("%s", e.what()); + return; + } + + return; + } + + private: + sensor_msgs::Image sci_cam_image_msg_; + sensor_msgs::CompressedImage sci_cam_image_msg_compressed_; + geometry_msgs::PoseStamped sci_cam_pose_msg_; + sensor_msgs::CameraInfo sci_cam_info_msg_; + + std::vector compression_params_; + + bool continuousPictureTaking_; + bool takeSinglePicture_; + std::mutex sci_cam_image_lock; + + ros::Publisher pub_sci_cam_image_; + ros::Publisher pub_sci_cam_pose_; + ros::Publisher pub_sci_cam_info_; + ros::Subscriber dds_cmd_sub_; + ros::Publisher pub_sci_cam_ack_; + std::shared_ptr sensor_; + event::ConnectionPtr update_; + double rate_; +}; + +GZ_REGISTER_SENSOR_PLUGIN(GazeboSensorPluginSciCam) + +} // namespace gazebo diff --git a/isaac/config/simulation/simulation.config b/isaac/config/simulation/simulation.config new file mode 100644 index 00000000..79150956 --- /dev/null +++ b/isaac/config/simulation/simulation.config @@ -0,0 +1,35 @@ +-- Copyright (c) 2021, 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. + +-- Unless you are on a high performance machine, ie. not in a virtualized +-- environment, with a decent processor and graphics card, enabling any +-- camera rendering (wide angle or depth) will consume significant resources. + +-- If we are running a faster-than-realtime simulation, then we are likely +-- going to need to disable our cameras to achieve the required performance. + +disable_cameras_on_speedup = true; + +-- Increasing the camera sample rate incurs a high rendering and serialization +-- cost. Setting the rate to zero effectively disables the camera. This rate +-- only applies to the sci cam when it is in continuous picture taking mode. + +sci_cam_rate = 1.0; + +-- Set to true to enable sci cam continous picture taking at startup +sci_cam_continuous_picture_taking = false; From ec567e9f3889b1059ca7dc500c8019d3cc3bbaa5 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Tue, 22 Mar 2022 14:16:29 -0700 Subject: [PATCH 3/6] output pano survey standalone (#43) * script that exports relative panorama ready * chasnging launchfile name --- astrobee/behaviors/inspection/CMakeLists.txt | 6 + .../inspection/launch/export_panorama.launch | 63 ++++++ .../behaviors/inspection/src/inspection.cc | 3 + .../inspection/tools/export_panorama.cc | 203 ++++++++++++++++++ .../inspection/tools/inspection_tool.cc | 2 +- 5 files changed, 276 insertions(+), 1 deletion(-) create mode 100644 astrobee/behaviors/inspection/launch/export_panorama.launch create mode 100644 astrobee/behaviors/inspection/tools/export_panorama.cc diff --git a/astrobee/behaviors/inspection/CMakeLists.txt b/astrobee/behaviors/inspection/CMakeLists.txt index c5a10e8d..03499531 100644 --- a/astrobee/behaviors/inspection/CMakeLists.txt +++ b/astrobee/behaviors/inspection/CMakeLists.txt @@ -78,6 +78,12 @@ add_dependencies(sci_cam_tool ${catkin_EXPORTED_TARGETS}) target_link_libraries(sci_cam_tool gflags ${catkin_LIBRARIES}) +## Declare a C++ executable: export_panorama +add_executable(export_panorama tools/export_panorama.cc) +add_dependencies(export_panorama ${catkin_EXPORTED_TARGETS}) +target_link_libraries(export_panorama + inspection gflags ${catkin_LIBRARIES}) + ############# ## Install ## ############# diff --git a/astrobee/behaviors/inspection/launch/export_panorama.launch b/astrobee/behaviors/inspection/launch/export_panorama.launch new file mode 100644 index 00000000..e31b02cb --- /dev/null +++ b/astrobee/behaviors/inspection/launch/export_panorama.launch @@ -0,0 +1,63 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/astrobee/behaviors/inspection/src/inspection.cc b/astrobee/behaviors/inspection/src/inspection.cc index a29da70e..1ada84f7 100644 --- a/astrobee/behaviors/inspection/src/inspection.cc +++ b/astrobee/behaviors/inspection/src/inspection.cc @@ -92,6 +92,7 @@ namespace inspection { tf_target_to_sci_cam.transform.rotation.w); } catch (tf2::TransformException &ex) { ROS_ERROR("ERROR getting target to sci_cam transform: %s", ex.what()); + target_to_scicam_rot_ = tf2::Quaternion(0, 0, 0, 1); } // Parameters Panorama survey @@ -338,6 +339,8 @@ namespace inspection { tf_sci_cam_to_body.transform.rotation.w)); } catch (tf2::TransformException &ex) { ROS_ERROR("ERROR getting sci_cam transform: %s", ex.what()); + sci_cam_to_body.setOrigin(tf2::Vector3(0, 0, 0)); + sci_cam_to_body.setRotation(tf2::Quaternion(0, 0, 0, 1)); } tf2::Transform target_to_sci_cam; diff --git a/astrobee/behaviors/inspection/tools/export_panorama.cc b/astrobee/behaviors/inspection/tools/export_panorama.cc new file mode 100644 index 00000000..d3b57f65 --- /dev/null +++ b/astrobee/behaviors/inspection/tools/export_panorama.cc @@ -0,0 +1,203 @@ +/* Copyright (c) 2021, 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. + */ + +// Command line flags (not used yet) +#include +#include + +// ROS includes +#include +#include + +// FSW includes +#include +#include +#include +#include +// #include +#include + +// C++ STL includes +#include +#include +#include +#include +#include +#include +#include + +#define DEG2RAD 3.1415/180.0 + +// Include inspection lib +#include "inspection/inspection.h" + +// Robot namespace +DEFINE_string(ns, "", "Robot namespace"); + +// Configurable Parameters +DEFINE_string(camera, "sci_cam", "Camera to use"); +DEFINE_double(tilt_max, 90.0, "Panorama: maximum tilt"); +DEFINE_double(tilt_min, -90.0, "Panorama: minimum tilt"); +DEFINE_double(pan_max, 180.0, "Panorama: maximum pan"); +DEFINE_double(pan_min, -180.0, "Panorama: minimum pan"); +DEFINE_double(overlap, 0.5, "Panorama: overlap between images"); + +// Plan files +DEFINE_string(panorama_poses, "/resources/scicam_panorama.txt", "Panorama poses list to map"); +DEFINE_string(panorama_out, "/resources/pano_out.txt", "Panorama poses output"); + + +bool has_only_whitespace_or_comments(const std::string & str) { + for (std::string::const_iterator it = str.begin(); it != str.end(); it++) { + if (*it == '#') return true; // No need to check further + if (*it != ' ' && *it != '\t' && *it != '\n' && *it != '\r') return false; + } + return true; +} + +void ReadFile(std::string file, isaac_msgs::InspectionGoal &goal) { + geometry_msgs::Pose pose; + goal.inspect_poses.header.frame_id = FLAGS_camera; + // Read file + std::ifstream ifs((file).c_str()); + if (!ifs.is_open()) { + std::cout << "Could not open file: " << file << std::endl; + return; + } + std::string line; + tf2::Quaternion quat_robot; + while (getline(ifs, line)) { + if (has_only_whitespace_or_comments(line)) continue; + + std::istringstream is(line); + double origin_x, origin_y, origin_z; + double euler_roll, euler_pitch, euler_yaw; + double quat_x, quat_y, quat_z, quat_w; + if ((is >> origin_x >> origin_y >> origin_z >> quat_x >> quat_y >> quat_z >> quat_w)) { + // Position + pose.position.x = origin_x; + pose.position.y = origin_y; + pose.position.z = origin_z; + + // Orientation + pose.orientation.x = quat_x; + pose.orientation.y = quat_y; + pose.orientation.z = quat_z; + pose.orientation.w = quat_w; + goal.inspect_poses.poses.push_back(pose); + + } else { + std::istringstream is(line); + if ((is >> origin_x >> origin_y >> origin_z >> euler_roll >> euler_pitch >> euler_yaw)) { + // Position + pose.position.x = origin_x; + pose.position.y = origin_y; + pose.position.z = origin_z; + + quat_robot.setRPY(euler_roll * DEG2RAD, + euler_pitch * DEG2RAD, + euler_yaw * DEG2RAD); + // Orientation + pose.orientation.x = quat_robot.x(); + pose.orientation.y = quat_robot.y(); + pose.orientation.z = quat_robot.z(); + pose.orientation.w = quat_robot.w(); + goal.inspect_poses.poses.push_back(pose); + + } else { + std::cout << "Ignoring invalid line: " << line << std::endl; + continue; + } + } + } +} + // Callback to handle reconfiguration requests + bool ReconfigureCallback(dynamic_reconfigure::Config & config) { + // if (cfg_.Reconfigure(config)) { + // // inspection_->ReadParam(); + // return true; + // } + return false; + } + + +int main(int argc, char *argv[]) { + // Gather some data from the command + google::SetUsageMessage("Usage: rosrun inspection export_panorama . "); + google::SetVersionString("0.1.0"); + google::ParseCommandLineFlags(&argc, &argv, true); + + // Initialize a ros node + ros::init(argc, argv, "panorama_export", ros::init_options::AnonymousName); + + // Create a node handle + std::string ns = std::string("/") + FLAGS_ns; + ros::NodeHandle nh; + + // Set the config path to ISAAC + ff_util::ConfigServer cfg_; + char *path = getenv("CUSTOM_CONFIG_DIR"); + if (path != NULL) + cfg_.SetPath(path); + // Grab some configuration parameters for this node from the LUA config reader + cfg_.Initialize(&nh, "behaviors/inspection.config"); + if (!cfg_.Listen(boost::bind(&ReconfigureCallback, _1))) + return 0; + // Set parameters from cmd line + cfg_.Set("pan_min", FLAGS_pan_min); + cfg_.Set("pan_max", FLAGS_pan_max); + cfg_.Set("tilt_min", FLAGS_tilt_min); + cfg_.Set("tilt_max", FLAGS_tilt_max); + cfg_.Set("overlap", FLAGS_overlap); + + // Initiate inspection library + inspection::Inspection inspection_(&nh, &cfg_); + isaac_msgs::InspectionGoal goal; + + // Read file + // std::cout << "Reading: " << FLAGS_panorama_poses << std::endl; + std::string path_inspection = std::string(ros::package::getPath("inspection")); + ReadFile(path_inspection + FLAGS_panorama_poses, goal); + + // ROS_ERROR("Generate Panorama"); + inspection_.GeneratePanoramaSurvey(goal.inspect_poses); + + // Write in file + std::ofstream myfile; + std::string path_output = ros::package::getPath("inspection") + FLAGS_panorama_out; + myfile.open(path_output); + for (int i = 0; i < goal.inspect_poses.poses.size(); i++) { + tf2::Quaternion quat(goal.inspect_poses.poses[i].orientation.x, goal.inspect_poses.poses[i].orientation.y, + goal.inspect_poses.poses[i].orientation.z, goal.inspect_poses.poses[i].orientation.w); + double roll, pitch, yaw; + tf2::Matrix3x3(quat).getRPY(roll, pitch, yaw); + + myfile << goal.inspect_poses.poses[i].position.x << " " << goal.inspect_poses.poses[i].position.y << " " + << goal.inspect_poses.poses[i].position.z << " " << round(roll / (DEG2RAD)*10.0) / 10.0 << " " + << round(pitch / (DEG2RAD)*10.0) / 10.0 << " " << round(yaw / (DEG2RAD)*10.0) / 10.0 << "\n"; + } + myfile.close(); + + + // Finish commandline flags + google::ShutDownCommandLineFlags(); + // Make for great success + return 0; +} + diff --git a/astrobee/behaviors/inspection/tools/inspection_tool.cc b/astrobee/behaviors/inspection/tools/inspection_tool.cc index 9b45b29c..7b9f6ee1 100644 --- a/astrobee/behaviors/inspection/tools/inspection_tool.cc +++ b/astrobee/behaviors/inspection/tools/inspection_tool.cc @@ -229,7 +229,7 @@ void ResultCallback(ff_util::FreeFlyerActionState::Enum code, void SendGoal(ff_util::FreeFlyerActionClient *client) { // Prepare the goal isaac_msgs::InspectionGoal goal; - std::string path = std::string(ros::package::getPath("inspection")); + std::string path = std::string(ros::package::getPath("inspection")); if (FLAGS_pause) { goal.command = isaac_msgs::InspectionGoal::PAUSE; } else if (FLAGS_resume) { From 5be6863707eb978528dfd63b10c7f62027630a53 Mon Sep 17 00:00:00 2001 From: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Wed, 23 Mar 2022 09:40:44 -0700 Subject: [PATCH 4/6] isaac6 surveys (#42) * adding surveys and organizing folder * adding panorama surveys --- ...ent_granite.txt => inspection_granite.txt} | 0 .../{vent_jpm.txt => inspection_iss.txt} | 0 .../resources/{ => isaac3}/survey_bay_5.txt | 0 .../{ => isaac3}/survey_bay_5_limited.txt | 0 .../resources/{ => isaac3}/survey_bay_6.txt | 0 .../resources/{ => isaac3}/survey_bay_7.txt | 0 .../resources/isaac6/calibration_survey.txt | 20 +++++++ .../resources/isaac6/jem_bay5_agg_pano.txt | 29 ++++++++++ .../isaac6/jem_bay5_std_panorama.txt | 54 +++++++++++++++++++ .../isaac6/jem_bay6_agg_panorama.txt | 29 ++++++++++ .../isaac6/jem_bay6_std_panorama.txt | 54 +++++++++++++++++++ .../isaac6/jem_bay7_agg_panorama.txt | 29 ++++++++++ .../isaac6/jem_bay7_std_panorama.txt | 54 +++++++++++++++++++ .../resources/isaac6/node2_std_panorama.txt | 2 + .../inspection/resources/panorama_iss.txt | 2 + .../{ => soundsee_data3}/scicam_panorama.txt | 0 .../scicam_panorama_geometry.txt | 0 .../soundsee_background_noise.txt | 0 .../soundsee_data3_bay6_deck.txt | 0 .../soundsee_panorama.txt | 0 .../soundsee_panorama_geometry.txt | 0 .../inspection/resources/vent_jpm_all.txt | 15 ------ ...ifi_granite.txt => volumetric_granite.txt} | 0 .../{wifi_jpm.txt => volumetric_iss.txt} | 0 24 files changed, 273 insertions(+), 15 deletions(-) rename astrobee/behaviors/inspection/resources/{vent_granite.txt => inspection_granite.txt} (100%) rename astrobee/behaviors/inspection/resources/{vent_jpm.txt => inspection_iss.txt} (100%) rename astrobee/behaviors/inspection/resources/{ => isaac3}/survey_bay_5.txt (100%) rename astrobee/behaviors/inspection/resources/{ => isaac3}/survey_bay_5_limited.txt (100%) rename astrobee/behaviors/inspection/resources/{ => isaac3}/survey_bay_6.txt (100%) rename astrobee/behaviors/inspection/resources/{ => isaac3}/survey_bay_7.txt (100%) create mode 100644 astrobee/behaviors/inspection/resources/isaac6/calibration_survey.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay5_agg_pano.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay5_std_panorama.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay6_agg_panorama.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay6_std_panorama.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay7_agg_panorama.txt create mode 100755 astrobee/behaviors/inspection/resources/isaac6/jem_bay7_std_panorama.txt create mode 100644 astrobee/behaviors/inspection/resources/isaac6/node2_std_panorama.txt create mode 100644 astrobee/behaviors/inspection/resources/panorama_iss.txt rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/scicam_panorama.txt (100%) rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/scicam_panorama_geometry.txt (100%) rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/soundsee_background_noise.txt (100%) rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/soundsee_data3_bay6_deck.txt (100%) rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/soundsee_panorama.txt (100%) rename astrobee/behaviors/inspection/resources/{ => soundsee_data3}/soundsee_panorama_geometry.txt (100%) delete mode 100644 astrobee/behaviors/inspection/resources/vent_jpm_all.txt rename astrobee/behaviors/inspection/resources/{wifi_granite.txt => volumetric_granite.txt} (100%) rename astrobee/behaviors/inspection/resources/{wifi_jpm.txt => volumetric_iss.txt} (100%) diff --git a/astrobee/behaviors/inspection/resources/vent_granite.txt b/astrobee/behaviors/inspection/resources/inspection_granite.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/vent_granite.txt rename to astrobee/behaviors/inspection/resources/inspection_granite.txt diff --git a/astrobee/behaviors/inspection/resources/vent_jpm.txt b/astrobee/behaviors/inspection/resources/inspection_iss.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/vent_jpm.txt rename to astrobee/behaviors/inspection/resources/inspection_iss.txt diff --git a/astrobee/behaviors/inspection/resources/survey_bay_5.txt b/astrobee/behaviors/inspection/resources/isaac3/survey_bay_5.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/survey_bay_5.txt rename to astrobee/behaviors/inspection/resources/isaac3/survey_bay_5.txt diff --git a/astrobee/behaviors/inspection/resources/survey_bay_5_limited.txt b/astrobee/behaviors/inspection/resources/isaac3/survey_bay_5_limited.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/survey_bay_5_limited.txt rename to astrobee/behaviors/inspection/resources/isaac3/survey_bay_5_limited.txt diff --git a/astrobee/behaviors/inspection/resources/survey_bay_6.txt b/astrobee/behaviors/inspection/resources/isaac3/survey_bay_6.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/survey_bay_6.txt rename to astrobee/behaviors/inspection/resources/isaac3/survey_bay_6.txt diff --git a/astrobee/behaviors/inspection/resources/survey_bay_7.txt b/astrobee/behaviors/inspection/resources/isaac3/survey_bay_7.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/survey_bay_7.txt rename to astrobee/behaviors/inspection/resources/isaac3/survey_bay_7.txt diff --git a/astrobee/behaviors/inspection/resources/isaac6/calibration_survey.txt b/astrobee/behaviors/inspection/resources/isaac6/calibration_survey.txt new file mode 100644 index 00000000..ed824afe --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/calibration_survey.txt @@ -0,0 +1,20 @@ +# Calibration survey +11 -10.0 4.4 0.0 30.0 180.0 +11 -9.5 4.4 0.0 30.0 180.0 +11 -9.0 4.4 0.0 30.0 180.0 +11 -8.5 4.4 0.0 30.0 180.0 +11 -8.0 4.4 0.0 30.0 180.0 +11 -8.0 4.4 0.0 0.0 180.0 +11 -8.5 4.4 0.0 0.0 180.0 +11 -9.0 4.4 0.0 0.0 180.0 +11 -9.5 4.4 0.0 0.0 180.0 +11 -10.0 4.4 0.0 0.0 180.0 +11 -10.0 4.9 0.0 0.0 180.0 +11 -9.5 4.9 0.0 0.0 180.0 +11 -9.0 4.9 0.0 0.0 180.0 +11 -8.5 4.9 0.0 0.0 180.0 +11 -8.0 4.9 0.0 0.0 180.0 +11 -8.0 5.4 0.0 0.0 180.0 +11 -8.5 5.4 0.0 0.0 180.0 +11 -9.0 5.4 0.0 0.0 180.0 +11 -9.5 5.4 0.0 0.0 180.0 \ No newline at end of file diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_agg_pano.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_agg_pano.txt new file mode 100755 index 00000000..f2289308 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_agg_pano.txt @@ -0,0 +1,29 @@ +10.916 -8.0176 4.94723 -0.073737 0.302644 0.224941 0.92324 +10.8426 -8.04893 4.896 0 0 0.212172 0.977232 +10.8151 -8.06989 4.80751 0.073737 -0.302644 0.224941 0.92324 +10.8776 -8.10424 4.71611 0.2744 -0.524563 0.373565 0.714134 +10.9191 -8.14875 4.80751 0.196571 -0.241641 0.599655 0.737146 +10.9221 -8.11466 4.896 0 0 0.617936 0.786229 +10.9422 -8.03747 4.94723 -0.196571 0.241641 0.599655 0.737146 +10.9821 -7.95374 4.94131 -0.2744 0.524563 0.373565 0.714134 +10.9079 -7.96262 4.94131 -0.553426 0.210194 0.753427 0.286156 +10.9741 -8.02969 4.94723 -0.281299 0.133796 0.858125 0.408156 +11.0229 -8.09278 4.896 0 0 0.899416 0.437093 +11.0459 -8.11786 4.80751 0.281299 -0.133796 0.858125 0.408156 +11.0449 -8.08424 4.71611 0.553426 -0.210194 0.753427 0.286156 +11.1019 -8.00001 4.80751 0.311497 -1.44307e-05 0.950247 4.40219e-05 +11.068 -8.00001 4.896 0 0 1 4.63268e-05 +10.9883 -8 4.94723 -0.311497 1.44307e-05 0.950247 4.40219e-05 +10.9079 -8.03738 4.94131 -0.553445 -0.210143 0.753454 -0.286086 +10.9742 -7.97032 4.94723 -0.281311 -0.13377 0.858163 -0.408076 +11.0229 -7.90724 4.896 0 0 0.899457 -0.43701 +11.0459 -7.88215 4.80751 0.281311 0.13377 0.858163 -0.408076 +11.0449 -7.91578 4.71611 0.553445 0.210143 0.753454 -0.286086 +10.8776 -7.89575 4.71611 -0.274448 -0.524538 -0.373631 0.7141 +10.9191 -7.85124 4.80751 -0.196593 -0.241623 -0.599723 0.73709 +10.9221 -7.88534 4.896 3.53047e-17 -3.5308e-17 -0.618008 0.786171 +10.9422 -7.96253 4.94723 0.196593 0.241623 -0.599723 0.73709 +10.9821 -8.04626 4.94131 0.274448 0.524538 -0.373631 0.7141 +10.916 -7.98239 4.94723 0.073765 0.302637 -0.225026 0.923219 +10.8426 -7.95105 4.896 0 0 -0.212262 0.977213 +10.8151 -7.93009 4.80751 -0.073765 -0.302637 -0.225026 0.923219 diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_std_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_std_panorama.txt new file mode 100755 index 00000000..84a78674 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay5_std_panorama.txt @@ -0,0 +1,54 @@ +10.9423 -8.00491 4.95184 -0.112849 0.386047 0.256882 0.878774 +10.8877 -8.03071 4.93535 -0.0466766 0.200115 0.222304 0.953077 +10.8436 -8.05098 4.896 0 0 0.221542 0.975151 +10.8185 -8.06488 4.84043 0.0466766 -0.200115 0.222304 0.953077 +10.8232 -8.08107 4.77804 0.112849 -0.386047 0.256882 0.878774 +10.8757 -8.10539 4.71935 0.267721 -0.51649 0.374309 0.722119 +10.9103 -8.14519 4.77804 0.244024 -0.319718 0.555481 0.727787 +10.8873 -8.13257 4.84043 0.109943 -0.173601 0.523619 0.8268 +10.8908 -8.1021 4.896 -1.60189e-17 -1.60174e-17 0.499315 0.866421 +10.9203 -8.06276 4.93535 -0.109943 0.173601 0.523619 0.8268 +10.9476 -8.0088 4.95184 -0.244024 0.319718 0.555481 0.727787 +10.9804 -7.95692 4.94269 -0.267721 0.51649 0.374309 0.722119 +10.9257 -7.95319 4.94269 -0.497077 0.302244 0.694977 0.422576 +10.9541 -8.00814 4.95184 -0.342606 0.210686 0.779887 0.479593 +10.9658 -8.06761 4.93535 -0.160957 0.12774 0.766581 0.608382 +10.9587 -8.11768 4.896 5.35266e-17 -2.80409e-17 0.732658 0.680597 +10.9833 -8.14281 4.84043 0.160957 -0.12774 0.766581 0.608382 +11.0179 -8.13433 4.77804 0.342606 -0.210686 0.779887 0.479593 +11.0094 -8.11451 4.71935 0.497077 -0.302244 0.694977 0.422576 +11.0905 -8.0541 4.77804 0.395428 -0.0735139 0.900127 0.167343 +11.0649 -8.09117 4.84043 0.194033 -0.0676442 0.924113 0.322165 +11.0235 -8.09231 4.896 9.24325e-17 -1.54087e-17 0.90081 0.434213 +11.0044 -8.04316 4.93535 -0.194033 0.0676442 0.924113 0.322165 +10.9585 -8.00328 4.95184 -0.395428 0.0735139 0.900127 0.167343 +11.0194 -8.00001 4.93535 -0.205487 9.51953e-06 0.97866 4.53382e-05 +11.0627 -8.03482 4.896 2.80697e-17 -2.80723e-17 0.988808 0.149193 +11.0966 -8.00001 4.84043 0.205487 -9.51953e-06 0.97866 4.53382e-05 +11.0628 -7.96521 4.896 1.40347e-17 -1.4036e-17 0.988822 -0.149102 +10.8973 -8 4.94269 -0.581753 2.69508e-05 0.813365 3.76806e-05 +10.9585 -7.99672 4.95184 -0.395434 -0.0734772 0.900143 -0.167259 +11.0044 -7.95685 4.93535 -0.19404 -0.0676262 0.924143 -0.32208 +11.0235 -7.9077 4.896 3.08104e-17 -3.08133e-17 0.90085 -0.43413 +11.0649 -7.90885 4.84043 0.19404 0.0676262 0.924143 -0.32208 +11.0905 -7.94593 4.77804 0.395434 0.0734772 0.900143 -0.167259 +11.079 -8.00001 4.71935 0.581753 -2.69508e-05 0.813365 3.76806e-05 +11.0094 -7.8855 4.71935 0.497105 0.302198 0.695016 -0.422511 +11.0179 -7.86568 4.77804 0.342625 0.210654 0.779931 -0.479521 +10.9833 -7.85719 4.84043 -0.160969 -0.127725 -0.766637 0.608311 +10.9587 -7.88232 4.896 0 0 -0.732721 0.680529 +10.9658 -7.93239 4.93535 0.160969 0.127725 -0.766637 0.608311 +10.9541 -7.99186 4.95184 -0.342625 -0.210654 0.779931 -0.479521 +10.9257 -8.0468 4.94269 -0.497105 -0.302198 0.695016 -0.422511 +10.9804 -8.04309 4.94269 0.267769 0.516465 -0.374376 0.722084 +10.9476 -7.9912 4.95184 0.244053 0.319696 -0.555548 0.727736 +10.9203 -7.93723 4.93535 0.109959 0.173591 -0.523696 0.826752 +10.8909 -7.89789 4.896 0 0 -0.499395 0.866374 +10.8873 -7.86742 4.84043 -0.109959 -0.173591 -0.523696 0.826752 +10.9103 -7.8548 4.77804 -0.244053 -0.319696 -0.555548 0.727736 +10.8757 -7.89459 4.71935 -0.267769 -0.516465 -0.374376 0.722084 +10.8232 -7.9189 4.77804 -0.112885 -0.386037 -0.256964 0.878751 +10.8185 -7.9351 4.84043 -0.0466951 -0.200111 -0.222392 0.953057 +10.8436 -7.949 4.896 0 0 -0.221632 0.97513 +10.8877 -7.96927 4.93535 0.0466951 0.200111 -0.222392 0.953057 +10.9423 -7.99509 4.95184 0.112885 0.386037 -0.256964 0.878751 diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_agg_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_agg_panorama.txt new file mode 100755 index 00000000..e8caa668 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_agg_panorama.txt @@ -0,0 +1,29 @@ +10.916 -9.0176 4.94723 -0.073737 0.302644 0.224941 0.92324 +10.8426 -9.04893 4.896 0 0 0.212172 0.977232 +10.8151 -9.06989 4.80751 0.073737 -0.302644 0.224941 0.92324 +10.8776 -9.10424 4.71611 0.2744 -0.524563 0.373565 0.714134 +10.9191 -9.14875 4.80751 0.196571 -0.241641 0.599655 0.737146 +10.9221 -9.11466 4.896 0 0 0.617936 0.786229 +10.9422 -9.03747 4.94723 -0.196571 0.241641 0.599655 0.737146 +10.9821 -8.95374 4.94131 -0.2744 0.524563 0.373565 0.714134 +10.9079 -8.96262 4.94131 -0.553426 0.210194 0.753427 0.286156 +10.9741 -9.02969 4.94723 -0.281299 0.133796 0.858125 0.408156 +11.0229 -9.09278 4.896 0 0 0.899416 0.437093 +11.0459 -9.11786 4.80751 0.281299 -0.133796 0.858125 0.408156 +11.0449 -9.08424 4.71611 0.553426 -0.210194 0.753427 0.286156 +11.1019 -9.00001 4.80751 0.311497 -1.44307e-05 0.950247 4.40219e-05 +11.068 -9.00001 4.896 0 0 1 4.63268e-05 +10.9883 -9 4.94723 -0.311497 1.44307e-05 0.950247 4.40219e-05 +10.9079 -9.03738 4.94131 -0.553445 -0.210143 0.753454 -0.286086 +10.9742 -8.97032 4.94723 -0.281311 -0.13377 0.858163 -0.408076 +11.0229 -8.90724 4.896 0 0 0.899457 -0.43701 +11.0459 -8.88215 4.80751 0.281311 0.13377 0.858163 -0.408076 +11.0449 -8.91578 4.71611 0.553445 0.210143 0.753454 -0.286086 +10.8776 -8.89575 4.71611 -0.274448 -0.524538 -0.373631 0.7141 +10.9191 -8.85124 4.80751 -0.196593 -0.241623 -0.599723 0.73709 +10.9221 -8.88534 4.896 3.53047e-17 -3.5308e-17 -0.618008 0.786171 +10.9422 -8.96253 4.94723 0.196593 0.241623 -0.599723 0.73709 +10.9821 -9.04626 4.94131 0.274448 0.524538 -0.373631 0.7141 +10.916 -8.98239 4.94723 0.073765 0.302637 -0.225026 0.923219 +10.8426 -8.95105 4.896 0 0 -0.212262 0.977213 +10.8151 -8.93009 4.80751 -0.073765 -0.302637 -0.225026 0.923219 diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_std_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_std_panorama.txt new file mode 100755 index 00000000..ab3664b0 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay6_std_panorama.txt @@ -0,0 +1,54 @@ +10.9423 -9.00491 4.95184 -0.112849 0.386047 0.256882 0.878774 +10.8877 -9.03071 4.93535 -0.0466766 0.200115 0.222304 0.953077 +10.8436 -9.05098 4.896 0 0 0.221542 0.975151 +10.8185 -9.06488 4.84043 0.0466766 -0.200115 0.222304 0.953077 +10.8232 -9.08107 4.77804 0.112849 -0.386047 0.256882 0.878774 +10.8757 -9.10539 4.71935 0.267721 -0.51649 0.374309 0.722119 +10.9103 -9.14519 4.77804 0.244024 -0.319718 0.555481 0.727787 +10.8873 -9.13257 4.84043 0.109943 -0.173601 0.523619 0.8268 +10.8908 -9.1021 4.896 -1.60189e-17 -1.60174e-17 0.499315 0.866421 +10.9203 -9.06276 4.93535 -0.109943 0.173601 0.523619 0.8268 +10.9476 -9.0088 4.95184 -0.244024 0.319718 0.555481 0.727787 +10.9804 -8.95692 4.94269 -0.267721 0.51649 0.374309 0.722119 +10.9257 -8.95319 4.94269 -0.497077 0.302244 0.694977 0.422576 +10.9541 -9.00814 4.95184 -0.342606 0.210686 0.779887 0.479593 +10.9658 -9.06761 4.93535 -0.160957 0.12774 0.766581 0.608382 +10.9587 -9.11768 4.896 5.35266e-17 -2.80409e-17 0.732658 0.680597 +10.9833 -9.14281 4.84043 0.160957 -0.12774 0.766581 0.608382 +11.0179 -9.13433 4.77804 0.342606 -0.210686 0.779887 0.479593 +11.0094 -9.11451 4.71935 0.497077 -0.302244 0.694977 0.422576 +11.0905 -9.0541 4.77804 0.395428 -0.0735139 0.900127 0.167343 +11.0649 -9.09117 4.84043 0.194033 -0.0676442 0.924113 0.322165 +11.0235 -9.09231 4.896 9.24325e-17 -1.54087e-17 0.90081 0.434213 +11.0044 -9.04316 4.93535 -0.194033 0.0676442 0.924113 0.322165 +10.9585 -9.00328 4.95184 -0.395428 0.0735139 0.900127 0.167343 +11.0194 -9.00001 4.93535 -0.205487 9.51953e-06 0.97866 4.53382e-05 +11.0627 -9.03482 4.896 2.80697e-17 -2.80723e-17 0.988808 0.149193 +11.0966 -9.00001 4.84043 0.205487 -9.51953e-06 0.97866 4.53382e-05 +11.0628 -8.96521 4.896 1.40347e-17 -1.4036e-17 0.988822 -0.149102 +10.8973 -9 4.94269 -0.581753 2.69508e-05 0.813365 3.76806e-05 +10.9585 -8.99672 4.95184 -0.395434 -0.0734772 0.900143 -0.167259 +11.0044 -8.95685 4.93535 -0.19404 -0.0676262 0.924143 -0.32208 +11.0235 -8.9077 4.896 3.08104e-17 -3.08133e-17 0.90085 -0.43413 +11.0649 -8.90885 4.84043 0.19404 0.0676262 0.924143 -0.32208 +11.0905 -8.94593 4.77804 0.395434 0.0734772 0.900143 -0.167259 +11.079 -9.00001 4.71935 0.581753 -2.69508e-05 0.813365 3.76806e-05 +11.0094 -8.8855 4.71935 0.497105 0.302198 0.695016 -0.422511 +11.0179 -8.86568 4.77804 0.342625 0.210654 0.779931 -0.479521 +10.9833 -8.85719 4.84043 -0.160969 -0.127725 -0.766637 0.608311 +10.9587 -8.88232 4.896 0 0 -0.732721 0.680529 +10.9658 -8.93239 4.93535 0.160969 0.127725 -0.766637 0.608311 +10.9541 -8.99186 4.95184 -0.342625 -0.210654 0.779931 -0.479521 +10.9257 -9.0468 4.94269 -0.497105 -0.302198 0.695016 -0.422511 +10.9804 -9.04309 4.94269 0.267769 0.516465 -0.374376 0.722084 +10.9476 -8.9912 4.95184 0.244053 0.319696 -0.555548 0.727736 +10.9203 -8.93723 4.93535 0.109959 0.173591 -0.523696 0.826752 +10.8909 -8.89789 4.896 0 0 -0.499395 0.866374 +10.8873 -8.86742 4.84043 -0.109959 -0.173591 -0.523696 0.826752 +10.9103 -8.8548 4.77804 -0.244053 -0.319696 -0.555548 0.727736 +10.8757 -8.89459 4.71935 -0.267769 -0.516465 -0.374376 0.722084 +10.8232 -8.9189 4.77804 -0.112885 -0.386037 -0.256964 0.878751 +10.8185 -8.9351 4.84043 -0.0466951 -0.200111 -0.222392 0.953057 +10.8436 -8.949 4.896 0 0 -0.221632 0.97513 +10.8877 -8.96927 4.93535 0.0466951 0.200111 -0.222392 0.953057 +10.9423 -8.99509 4.95184 0.112885 0.386037 -0.256964 0.878751 diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_agg_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_agg_panorama.txt new file mode 100755 index 00000000..ef8ee925 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_agg_panorama.txt @@ -0,0 +1,29 @@ +10.916 -10.0176 4.94723 -0.073737 0.302644 0.224941 0.92324 +10.8426 -10.0489 4.896 0 0 0.212172 0.977232 +10.8151 -10.0699 4.80751 0.073737 -0.302644 0.224941 0.92324 +10.8776 -10.1042 4.71611 0.2744 -0.524563 0.373565 0.714134 +10.9191 -10.1488 4.80751 0.196571 -0.241641 0.599655 0.737146 +10.9221 -10.1147 4.896 0 0 0.617936 0.786229 +10.9422 -10.0375 4.94723 -0.196571 0.241641 0.599655 0.737146 +10.9821 -9.95374 4.94131 -0.2744 0.524563 0.373565 0.714134 +10.9079 -9.96262 4.94131 -0.553426 0.210194 0.753427 0.286156 +10.9741 -10.0297 4.94723 -0.281299 0.133796 0.858125 0.408156 +11.0229 -10.0928 4.896 0 0 0.899416 0.437093 +11.0459 -10.1179 4.80751 0.281299 -0.133796 0.858125 0.408156 +11.0449 -10.0842 4.71611 0.553426 -0.210194 0.753427 0.286156 +11.1019 -10 4.80751 0.311497 -1.44307e-05 0.950247 4.40219e-05 +11.068 -10 4.896 0 0 1 4.63268e-05 +10.9883 -10 4.94723 -0.311497 1.44307e-05 0.950247 4.40219e-05 +10.9079 -10.0374 4.94131 -0.553445 -0.210143 0.753454 -0.286086 +10.9742 -9.97032 4.94723 -0.281311 -0.13377 0.858163 -0.408076 +11.0229 -9.90724 4.896 0 0 0.899457 -0.43701 +11.0459 -9.88215 4.80751 0.281311 0.13377 0.858163 -0.408076 +11.0449 -9.91578 4.71611 0.553445 0.210143 0.753454 -0.286086 +10.8776 -9.89575 4.71611 -0.274448 -0.524538 -0.373631 0.7141 +10.9191 -9.85124 4.80751 -0.196593 -0.241623 -0.599723 0.73709 +10.9221 -9.88534 4.896 3.53047e-17 -3.5308e-17 -0.618008 0.786171 +10.9422 -9.96253 4.94723 0.196593 0.241623 -0.599723 0.73709 +10.9821 -10.0463 4.94131 0.274448 0.524538 -0.373631 0.7141 +10.916 -9.98239 4.94723 0.073765 0.302637 -0.225026 0.923219 +10.8426 -9.95105 4.896 0 0 -0.212262 0.977213 +10.8151 -9.93009 4.80751 -0.073765 -0.302637 -0.225026 0.923219 diff --git a/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_std_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_std_panorama.txt new file mode 100755 index 00000000..8430ef78 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/jem_bay7_std_panorama.txt @@ -0,0 +1,54 @@ +10.9423 -10.0049 4.95184 -0.112849 0.386047 0.256882 0.878774 +10.8877 -10.0307 4.93535 -0.0466766 0.200115 0.222304 0.953077 +10.8436 -10.051 4.896 0 0 0.221542 0.975151 +10.8185 -10.0649 4.84043 0.0466766 -0.200115 0.222304 0.953077 +10.8232 -10.0811 4.77804 0.112849 -0.386047 0.256882 0.878774 +10.8757 -10.1054 4.71935 0.267721 -0.51649 0.374309 0.722119 +10.9103 -10.1452 4.77804 0.244024 -0.319718 0.555481 0.727787 +10.8873 -10.1326 4.84043 0.109943 -0.173601 0.523619 0.8268 +10.8908 -10.1021 4.896 -1.60189e-17 -1.60174e-17 0.499315 0.866421 +10.9203 -10.0628 4.93535 -0.109943 0.173601 0.523619 0.8268 +10.9476 -10.0088 4.95184 -0.244024 0.319718 0.555481 0.727787 +10.9804 -9.95692 4.94269 -0.267721 0.51649 0.374309 0.722119 +10.9257 -9.95319 4.94269 -0.497077 0.302244 0.694977 0.422576 +10.9541 -10.0081 4.95184 -0.342606 0.210686 0.779887 0.479593 +10.9658 -10.0676 4.93535 -0.160957 0.12774 0.766581 0.608382 +10.9587 -10.1177 4.896 5.35266e-17 -2.80409e-17 0.732658 0.680597 +10.9833 -10.1428 4.84043 0.160957 -0.12774 0.766581 0.608382 +11.0179 -10.1343 4.77804 0.342606 -0.210686 0.779887 0.479593 +11.0094 -10.1145 4.71935 0.497077 -0.302244 0.694977 0.422576 +11.0905 -10.0541 4.77804 0.395428 -0.0735139 0.900127 0.167343 +11.0649 -10.0912 4.84043 0.194033 -0.0676442 0.924113 0.322165 +11.0235 -10.0923 4.896 9.24325e-17 -1.54087e-17 0.90081 0.434213 +11.0044 -10.0432 4.93535 -0.194033 0.0676442 0.924113 0.322165 +10.9585 -10.0033 4.95184 -0.395428 0.0735139 0.900127 0.167343 +11.0194 -10 4.93535 -0.205487 9.51953e-06 0.97866 4.53382e-05 +11.0627 -10.0348 4.896 2.80697e-17 -2.80723e-17 0.988808 0.149193 +11.0966 -10 4.84043 0.205487 -9.51953e-06 0.97866 4.53382e-05 +11.0628 -9.96521 4.896 1.40347e-17 -1.4036e-17 0.988822 -0.149102 +10.8973 -10 4.94269 -0.581753 2.69508e-05 0.813365 3.76806e-05 +10.9585 -9.99672 4.95184 -0.395434 -0.0734772 0.900143 -0.167259 +11.0044 -9.95685 4.93535 -0.19404 -0.0676262 0.924143 -0.32208 +11.0235 -9.9077 4.896 3.08104e-17 -3.08133e-17 0.90085 -0.43413 +11.0649 -9.90885 4.84043 0.19404 0.0676262 0.924143 -0.32208 +11.0905 -9.94593 4.77804 0.395434 0.0734772 0.900143 -0.167259 +11.079 -10 4.71935 0.581753 -2.69508e-05 0.813365 3.76806e-05 +11.0094 -9.8855 4.71935 0.497105 0.302198 0.695016 -0.422511 +11.0179 -9.86568 4.77804 0.342625 0.210654 0.779931 -0.479521 +10.9833 -9.85719 4.84043 -0.160969 -0.127725 -0.766637 0.608311 +10.9587 -9.88232 4.896 0 0 -0.732721 0.680529 +10.9658 -9.93239 4.93535 0.160969 0.127725 -0.766637 0.608311 +10.9541 -9.99186 4.95184 -0.342625 -0.210654 0.779931 -0.479521 +10.9257 -10.0468 4.94269 -0.497105 -0.302198 0.695016 -0.422511 +10.9804 -10.0431 4.94269 0.267769 0.516465 -0.374376 0.722084 +10.9476 -9.9912 4.95184 0.244053 0.319696 -0.555548 0.727736 +10.9203 -9.93723 4.93535 0.109959 0.173591 -0.523696 0.826752 +10.8909 -9.89789 4.896 0 0 -0.499395 0.866374 +10.8873 -9.86742 4.84043 -0.109959 -0.173591 -0.523696 0.826752 +10.9103 -9.8548 4.77804 -0.244053 -0.319696 -0.555548 0.727736 +10.8757 -9.89459 4.71935 -0.267769 -0.516465 -0.374376 0.722084 +10.8232 -9.9189 4.77804 -0.112885 -0.386037 -0.256964 0.878751 +10.8185 -9.9351 4.84043 -0.0466951 -0.200111 -0.222392 0.953057 +10.8436 -9.949 4.896 0 0 -0.221632 0.97513 +10.8877 -9.96927 4.93535 0.0466951 0.200111 -0.222392 0.953057 +10.9423 -9.99509 4.95184 0.112885 0.386037 -0.256964 0.878751 diff --git a/astrobee/behaviors/inspection/resources/isaac6/node2_std_panorama.txt b/astrobee/behaviors/inspection/resources/isaac6/node2_std_panorama.txt new file mode 100644 index 00000000..70213770 --- /dev/null +++ b/astrobee/behaviors/inspection/resources/isaac6/node2_std_panorama.txt @@ -0,0 +1,2 @@ +# Panorama Node2 +10.0 0.0 4.8 0.0 0.0 0.0 \ No newline at end of file diff --git a/astrobee/behaviors/inspection/resources/panorama_iss.txt b/astrobee/behaviors/inspection/resources/panorama_iss.txt new file mode 100644 index 00000000..71e3eb8f --- /dev/null +++ b/astrobee/behaviors/inspection/resources/panorama_iss.txt @@ -0,0 +1,2 @@ +# Panorama +10.9 -9 5.0 0 0 180 \ No newline at end of file diff --git a/astrobee/behaviors/inspection/resources/scicam_panorama.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/scicam_panorama.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/scicam_panorama.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/scicam_panorama.txt diff --git a/astrobee/behaviors/inspection/resources/scicam_panorama_geometry.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/scicam_panorama_geometry.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/scicam_panorama_geometry.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/scicam_panorama_geometry.txt diff --git a/astrobee/behaviors/inspection/resources/soundsee_background_noise.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_background_noise.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/soundsee_background_noise.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_background_noise.txt diff --git a/astrobee/behaviors/inspection/resources/soundsee_data3_bay6_deck.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_data3_bay6_deck.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/soundsee_data3_bay6_deck.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_data3_bay6_deck.txt diff --git a/astrobee/behaviors/inspection/resources/soundsee_panorama.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_panorama.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/soundsee_panorama.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_panorama.txt diff --git a/astrobee/behaviors/inspection/resources/soundsee_panorama_geometry.txt b/astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_panorama_geometry.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/soundsee_panorama_geometry.txt rename to astrobee/behaviors/inspection/resources/soundsee_data3/soundsee_panorama_geometry.txt diff --git a/astrobee/behaviors/inspection/resources/vent_jpm_all.txt b/astrobee/behaviors/inspection/resources/vent_jpm_all.txt deleted file mode 100644 index 22940374..00000000 --- a/astrobee/behaviors/inspection/resources/vent_jpm_all.txt +++ /dev/null @@ -1,15 +0,0 @@ -# x y z roll pitch yaw -10.2067169189 -3.1288216114 4.82276105881 90 0.000000 0.000000 -11.971072 -9.193897 3.963823 0.000000 -40 0.000000 -11.921994 -8.174384 3.923862 0.000000 -40 0.000000 -11.921994 -7.197721 3.923862 0.000000 -40 0.000000 -11.921994 -6.196085 3.923862 0.000000 -40 0.000000 -11.921994 -5.182941 3.923862 0.000000 -40 0.000000 -11.921994 -4.200116 3.923862 0.000000 -40 0.000000 -11.921994 -3.201995 3.923862 0.000000 -40 0.000000 -9.942000 -9.189370 3.910500 0.000000 40 0.000000 -9.942000 -8.189623 3.910500 0.000000 40 0.000000 -9.942000 -7.145759 3.910500 0.000000 40 0.000000 -9.942000 -6.190303 3.910500 0.000000 40 0.000000 -9.942000 -5.215301 3.910500 0.000000 40 0.000000 -9.942000 -4.210000 3.910500 0.000000 40 0.000000 \ No newline at end of file diff --git a/astrobee/behaviors/inspection/resources/wifi_granite.txt b/astrobee/behaviors/inspection/resources/volumetric_granite.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/wifi_granite.txt rename to astrobee/behaviors/inspection/resources/volumetric_granite.txt diff --git a/astrobee/behaviors/inspection/resources/wifi_jpm.txt b/astrobee/behaviors/inspection/resources/volumetric_iss.txt similarity index 100% rename from astrobee/behaviors/inspection/resources/wifi_jpm.txt rename to astrobee/behaviors/inspection/resources/volumetric_iss.txt From 741c01b6a0debe67f03cf32a59ed5e67005fc89a Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 4 May 2022 12:05:43 -0700 Subject: [PATCH 5/6] Merge pull request #44 from rsoussan/soundsee_depth_tool commit 0a1a8d6f60f73caec321bbcf18a623e60f55c191 Merge: 5be6863 e19ab71 Author: Ryan Soussan Date: Mon Apr 4 13:38:34 2022 -0700 Merge pull request #44 from rsoussan/soundsee_depth_tool * Added soundsee depth tool * fixed td mesh writing * fixed depth print out * updated timestamp error * updated save depth data fcn * updated load sensor rays fcn * added output o abbreviation * loading multiple cams * updated test depth utils * added test at angle * added utils file for depth mesh utils * added output filename * added saving depth data * added file loading * added getdepth data fcn * added make groundtruth pose interplator fcn --- dense_map/geometry_mapper/CMakeLists.txt | 20 +- .../include/depth_from_mesh_utils.h | 73 +++++++ .../include/texture_processing.h | 2 +- dense_map/geometry_mapper/package.xml | 4 +- .../src/depth_from_mesh_utils.cc | 183 ++++++++++++++++ .../test/data/1234.22_nav_cam_world.txt | 4 + .../test/data/22222.1119_nav_cam_world.txt | 4 + .../test/data/313131.99_nav_cam_world.txt | 4 + .../geometry_mapper/test/data/sensor_rays.csv | 3 + .../geometry_mapper/test/data/timestamps.csv | 3 + .../test/test_depth_from_mesh_utils.cc | 202 ++++++++++++++++++ .../test/test_depth_from_mesh_utils.test | 22 ++ .../tools/get_depth_data_from_mesh.cc | 136 ++++++++++++ 13 files changed, 657 insertions(+), 3 deletions(-) create mode 100644 dense_map/geometry_mapper/include/depth_from_mesh_utils.h create mode 100644 dense_map/geometry_mapper/src/depth_from_mesh_utils.cc create mode 100644 dense_map/geometry_mapper/test/data/1234.22_nav_cam_world.txt create mode 100644 dense_map/geometry_mapper/test/data/22222.1119_nav_cam_world.txt create mode 100644 dense_map/geometry_mapper/test/data/313131.99_nav_cam_world.txt create mode 100644 dense_map/geometry_mapper/test/data/sensor_rays.csv create mode 100644 dense_map/geometry_mapper/test/data/timestamps.csv create mode 100644 dense_map/geometry_mapper/test/test_depth_from_mesh_utils.cc create mode 100644 dense_map/geometry_mapper/test/test_depth_from_mesh_utils.test create mode 100644 dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc diff --git a/dense_map/geometry_mapper/CMakeLists.txt b/dense_map/geometry_mapper/CMakeLists.txt index 18c12e73..efa8ab43 100644 --- a/dense_map/geometry_mapper/CMakeLists.txt +++ b/dense_map/geometry_mapper/CMakeLists.txt @@ -39,6 +39,7 @@ find_package(catkin REQUIRED COMPONENTS cv_bridge image_transport config_reader + localization_common ) # The quantity CMAKE_PREFIX_PATH can be set as an environmental @@ -117,7 +118,7 @@ set(TEXRECON_INCLUDE_DIRS catkin_package( INCLUDE_DIRS include LIBRARIES geometry_mapper - CATKIN_DEPENDS roscpp std_msgs nodelet camera interest_point sparse_mapping ff_util isaac_hw_msgs eigen_conversions rosbag cv_bridge image_transport config_reader + CATKIN_DEPENDS roscpp std_msgs nodelet camera interest_point sparse_mapping ff_util isaac_hw_msgs eigen_conversions rosbag cv_bridge image_transport config_reader localization_common # DEPENDS system_lib ) @@ -141,6 +142,7 @@ add_definitions(${PCL_DEFINITIONS}) # Declare a C++ library add_library(geometry_mapper_lib + src/depth_from_mesh_utils.cc src/dense_map_utils.cc src/dense_map_ros_utils.cc src/TinyEXIF.cc @@ -180,6 +182,10 @@ add_executable(camera_refiner_old tools/camera_refiner_old.cc) target_link_libraries(camera_refiner_old geometry_mapper_lib gflags glog) +add_executable(get_depth_data_from_mesh tools/get_depth_data_from_mesh.cc) +target_link_libraries(get_depth_data_from_mesh + geometry_mapper_lib gflags glog) + add_executable(extract_pc_from_bag tools/extract_pc_from_bag.cc) target_link_libraries(extract_pc_from_bag geometry_mapper_lib gflags glog) @@ -220,6 +226,18 @@ add_executable(streaming_mapper tools/streaming_mapper.cc) target_link_libraries(streaming_mapper geometry_mapper_lib gflags glog) +if(CATKIN_ENABLE_TESTING) + find_package(rostest REQUIRED) + add_rostest_gtest(test_depth_from_mesh_utils + test/test_depth_from_mesh_utils.test + test/test_depth_from_mesh_utils.cc + ) + target_link_libraries(test_depth_from_mesh_utils + ${PROJECT_NAME}_lib glog ${catkin_LIBRARIES} + ) + +endif() + ############# ## Install ## ############# diff --git a/dense_map/geometry_mapper/include/depth_from_mesh_utils.h b/dense_map/geometry_mapper/include/depth_from_mesh_utils.h new file mode 100644 index 00000000..7bbd318d --- /dev/null +++ b/dense_map/geometry_mapper/include/depth_from_mesh_utils.h @@ -0,0 +1,73 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform 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. + */ + +#ifndef DEPTH_FROM_MESH_UTILS_H_ +#define DEPTH_FROM_MESH_UTILS_H_ + +#include +#include + +#include +#include +#include + +#include + +#include + +#include +#include + +namespace dense_map { +typedef acc::BVHTree BVHTree; + +boost::optional Depth(const Eigen::Vector3d& sensor_t_ray, const Eigen::Isometry3d& world_T_sensor, + const BVHTree& bvh_tree, double min_depth = 1e-3, double max_depth = 1e2); + +localization_common::Time LoadTimestamp(const std::string& filename, const double timestamp_offset = 0.0); + +void LoadTimestampsAndPoses(const std::string& directory, const std::string& sensor_name, + std::vector& timestamps, std::vector& poses, + const Eigen::Isometry3d& poses_sensor_T_sensor = Eigen::Isometry3d::Identity(), + const double timestamp_offset = 0.0); + +// Assumes each timestamp is on a newline of the file +std::vector LoadTimestamps(const std::string& timestamps_filename); + +localization_common::PoseInterpolater MakePoseInterpolater( + const std::string& directory, const Eigen::Isometry3d& body_T_sensor, + const std::vector& groundtruth_sensor_names, + const std::vector& body_T_groundtruth_sensor_vec, const std::vector& timestamp_offsets); + +// Assumes each point is sampled from an y, z grid with an x offset of 1.0 +// Points are entered as "y z" values with new points on a newline in the file +std::vector LoadSensorRays(const std::string& sensor_rays_filename, int& rows); + +std::shared_ptr LoadMeshTree(const std::string& mesh_file); + +std::vector> GetDepthData( + const std::vector& timestamps, const std::vector& sensor_t_rays, + const localization_common::PoseInterpolater& groundtruth_pose_interpolater, const BVHTree& bvh_tree); + +void SaveDepthData(const std::vector& timestamps, + const std::vector& sensor_t_rays, + const std::vector>& depths, const std::string& output_filename, + const int rows); +} // namespace dense_map + +#endif // DEPTH_FROM_MESH_UTILS_H_ diff --git a/dense_map/geometry_mapper/include/texture_processing.h b/dense_map/geometry_mapper/include/texture_processing.h index 4bf09af0..4bd0fa62 100644 --- a/dense_map/geometry_mapper/include/texture_processing.h +++ b/dense_map/geometry_mapper/include/texture_processing.h @@ -159,7 +159,7 @@ inline IsaacTexturePatch::IsaacTexturePatch(int64_t label, std::vector(texture_patch.faces); texcoords = std::vector(texture_patch.texcoords); diff --git a/dense_map/geometry_mapper/package.xml b/dense_map/geometry_mapper/package.xml index 6708fb43..0c0d97ed 100644 --- a/dense_map/geometry_mapper/package.xml +++ b/dense_map/geometry_mapper/package.xml @@ -29,6 +29,7 @@ cv_bridge image_transport config_reader + localization_common roscpp std_msgs sensor_msgs @@ -43,7 +44,7 @@ rosbag cv_bridge image_transport - config_reader + localization_common roscpp std_msgs sensor_msgs @@ -59,6 +60,7 @@ cv_bridge image_transport config_reader + localization_common + + + + + + + + + + + + + + + + + + + + + diff --git a/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc b/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc new file mode 100644 index 00000000..d7b5f2d8 --- /dev/null +++ b/dense_map/geometry_mapper/tools/get_depth_data_from_mesh.cc @@ -0,0 +1,136 @@ +/* Copyright (c) 2017, United States Government, as represented by the + * Administrator of the National Aeronautics and Space Administration. + * + * All rights reserved. + * + * The Astrobee platform 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. + */ + +#include +#include +#include +#include + +#include +#include + +#include + +#include + +namespace dm = dense_map; +namespace fs = boost::filesystem; +namespace po = boost::program_options; +namespace lc = localization_common; +namespace mc = msg_conversions; + +int main(int argc, char** argv) { + std::string robot_config_file; + std::string world; + std::string sensor_frame; + std::string output_file; + po::options_description desc( + "Adds depth data to desired points at provided timestamps. Uses a provided mesh and sequence of groundtruth poses " + "to obtain the depth data."); + desc.add_options()("help,h", "produce help message")( + "timestamps-file", po::value()->required(), + "File containing a set of timestamps to generate depth data for. Each timestamp should be on a newline.")( + "sensor-rays-file", po::value()->required(), + "File containing a set of rays from the sensor frame to generate depth data for. Each point has an assumed x " + "offset of 1 meter from the sensor, so only y z pairs are required. Each point pair should be on a newline.")( + "groundtruth-directory", po::value()->required(), + "Directory containing groundtruth poses with timestamps as filenames.")( + "mesh", po::value()->required(), "Mesh used to provide depth data.")( + "config-path,c", po::value()->required(), "Path to astrobee config directory.")( + "robot-config-file,r", po::value(&robot_config_file)->default_value("config/robots/bumble.config"), + "Robot config file")("world,w", po::value(&world)->default_value("iss"), "World name")( + "output-file,o", po::value(&output_file)->default_value("depths.csv"), + "Output file containing timestamp, depth, x, y values on each line. Timestamps for which depth data was not " + "successfully loaded are not included in the output.") + // TODO(rsoussan): Add soundsee sensor trafo to astrobee configs! + ("sensor-frame,s", po::value(&sensor_frame)->default_value("soundsee"), + "Sensor frame to generate depth data in."); + + po::positional_options_description p; + p.add("timestamps-file", 1); + p.add("sensor-rays-file", 1); + p.add("groundtruth-directory", 1); + p.add("mesh", 1); + p.add("config-path", 1); + po::variables_map vm; + try { + po::store(po::command_line_parser(argc, argv).options(desc).positional(p).run(), vm); + if (vm.count("help") || (argc <= 1)) { + std::cout << desc << "\n"; + return 1; + } + po::notify(vm); + } catch (std::exception& e) { + std::cerr << "Error: " << e.what() << "\n"; + return 1; + } + + const std::string timestamps_file = vm["timestamps-file"].as(); + const std::string sensor_rays_file = vm["sensor-rays-file"].as(); + const std::string groundtruth_directory = vm["groundtruth-directory"].as(); + const std::string mesh_file = vm["mesh"].as(); + const std::string config_path = vm["config-path"].as(); + + // Only pass program name to free flyer so that boost command line options + // are ignored when parsing gflags. + int ff_argc = 1; + ff_common::InitFreeFlyerApplication(&ff_argc, &argv); + + if (!fs::exists(timestamps_file)) { + LOG(FATAL) << "Timestamps file " << timestamps_file << " not found."; + } + if (!fs::exists(sensor_rays_file)) { + LOG(FATAL) << "Sensor rays file " << sensor_rays_file << " not found."; + } + if (!fs::exists(groundtruth_directory)) { + LOG(FATAL) << "Groundtruth directory " << groundtruth_directory << " not found."; + } + if (!fs::exists(mesh_file)) { + LOG(FATAL) << "Mesh " << mesh_file << " not found."; + } + + lc::SetEnvironmentConfigs(config_path, world, robot_config_file); + config_reader::ConfigReader config; + config.AddFile("geometry.config"); + if (!config.ReadFiles()) { + LOG(FATAL) << "Failed to read config files."; + } + + // Needed later to write results in correct format + int mesh_rows; + const auto sensor_t_rays = dm::LoadSensorRays(sensor_rays_file, mesh_rows); + const auto query_timestamps = dm::LoadTimestamps(timestamps_file); + const auto body_T_sensor = mc::LoadEigenTransform(config, sensor_frame + "_transform"); + std::vector groundtruth_sensor_frames{"nav_cam", "sci_cam", "haz_cam"}; + // TODO(rsoussan): Add option to pass these as args? + std::vector timestamp_offsets{0, 0, 0}; + std::vector body_T_groundtruth_sensor_vec; + for (const auto& sensor_frame : groundtruth_sensor_frames) { + body_T_groundtruth_sensor_vec.emplace_back(mc::LoadEigenTransform(config, sensor_frame + "_transform")); + } + const auto groundtruth_pose_interpolater = dm::MakePoseInterpolater( + groundtruth_directory, body_T_sensor, groundtruth_sensor_frames, body_T_groundtruth_sensor_vec, timestamp_offsets); + const auto mesh_tree = dm::LoadMeshTree(mesh_file); + const auto depths = dm::GetDepthData(query_timestamps, sensor_t_rays, groundtruth_pose_interpolater, *mesh_tree); + int depth_count = 0; + for (const auto& depth : depths) { + if (depth) ++depth_count; + } + LOG(INFO) << "Got " << depth_count << " of " << depths.size() << " depths successfully."; + dm::SaveDepthData(query_timestamps, sensor_t_rays, depths, output_file, mesh_rows); +} From 61aac2d5fe8b79060a3a0f43739291bd595d14a4 Mon Sep 17 00:00:00 2001 From: Marina Moreira Date: Wed, 4 May 2022 12:09:18 -0700 Subject: [PATCH 6/6] Release 0.2.5 commit 38531855beffa747ae3b4eee4e82728290c5f60c Author: Marina Moreira <67443181+marinagmoreira@users.noreply.github.com> Date: Mon Apr 11 15:36:37 2022 -0700 bumping up version (#45) --- RELEASE.md | 6 ++++++ debian/changelog | 12 ++++++++++-- isaac.doxyfile | 2 +- 3 files changed, 17 insertions(+), 3 deletions(-) diff --git a/RELEASE.md b/RELEASE.md index 55540d42..112cfe28 100644 --- a/RELEASE.md +++ b/RELEASE.md @@ -1,5 +1,11 @@ # Releases +## Release 0.2.5 + + * Add sci cam plugin to isaac repo + * Soundsee depth tool + * Output standalone panorama survey + ## Release 0.2.4 * Inspection tool improvements diff --git a/debian/changelog b/debian/changelog index c7b1fd46..cf3ed97a 100644 --- a/debian/changelog +++ b/debian/changelog @@ -1,17 +1,25 @@ +isaac (0.2.5) testing; urgency=medium + + * Add sci cam plugin to isaac repo + * Soundsee depth tool + * Output standalone panorama survey + + -- ISAAC Flight Software Tue, 05 Apr 2022 20:02:48 -0700 + isaac (0.2.4) testing; urgency=medium * Inspection tool improvements * Better CI * Small bug fixes - -- Marina Moreira Fri, 11 Feb 2022 13:57:38 -0800 + -- ISAAC Flight Software Fri, 11 Feb 2022 13:57:38 -0800 isaac (0.2.3) testing; urgency=medium * Panorama picture taking mode supported * Cargo transport simulation support - -- Marina Moreira Mon, 13 Dec 2021 15:51:01 -0800 + -- ISAAC Flight Software Mon, 13 Dec 2021 15:51:01 -0800 isaac (0.2.2) testing; urgency=medium diff --git a/isaac.doxyfile b/isaac.doxyfile index 6842160e..0267d8e9 100644 --- a/isaac.doxyfile +++ b/isaac.doxyfile @@ -38,7 +38,7 @@ PROJECT_NAME = "ISAAC" # could be handy for archiving the generated documentation or if some version # control system is used. -PROJECT_NUMBER = 0.2.4 +PROJECT_NUMBER = 0.2.5 # Using the PROJECT_BRIEF tag one can provide an optional one line description # for a project that appears at the top of each page and should give viewer a