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/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/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/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/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
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) {
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/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/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