Morgan develop frcnn

astrobee/localization/cnn_object_localization/README.md

@@ -1,3 +1,22 @@
+# NOTE:
+
+NOTE: The code in this folder is in a fairly complete state. 
+Everything "works" and is reasonably well-documented. 
+A minimal C++ test of the model can be cross-compiled and installed to Astrobee.
+The only problem is that the Mask-RCNN model used is too large, and can't be loaded into Astrobee's memory.
+If you were to hypothetically slap another stick of RAM into Astrobee, the model would probably be able to run.
+
+See the `lightweight_cnn_object_localization` folder for a newer approach to this project. Some key differences include:
+
+- Using a keypoint-based pipeline instead of a mask-based pipeline
+- Using a model with MobileNetV3 instead of ResNet-50 as the backbone (orders of magnitude faster/smaller)
+- Torch 1.13.1 instead of Torch 1.5.0
+- Substantially less complete
+    - Essentially just a proof-of-concept that the model works and can run on the robot
+    - No integration of the model, either in simulation or on the robot hardware, for actual localization
+
+If you need to build this package, you probably want to remove the `CATKIN_IGNORE` file.
+
 # Overview
 
 This module contains the code for handrail pose estimation. 

astrobee/localization/cnn_object_localization/tools/pytorch_rcnn_training/README.md

@@ -18,7 +18,7 @@ The training for Mask-RCNN relies on features from modern Torch/TorchVision. How
 2. `python3 convert_mrcnn_to_torchscript.py -d "PATH/TO/IMAGES/FOLDER" -w "PATH/TO/CHECKPOINTS/FOLDER/handrail_mrcnn_finetune_ckpt_199.pth"`
 3. `python3 test_mrcnn_torchscript.py -d "PATH/TO/IMAGES/FOLDER" -o "PATH/TO/OUTPUTS/FOLDER" -w "PATH/TO/CHECKPOINTS/FOLDER/handrail_mrcnn_finetune_ckpt_199_torchscript.pt"`
 
-# Keypoint-RCNN for handrail detection
+# Keypoint-RCNN for handrail detection (EXPERIMENTAL)
 
 This tool also contains scripts for training and testing an experimental keypoint-based version of RCNN. Note that this does not use the actual Keypoint-RCNN architecture, but instead just trains a Mask-RCNN on data containing keypoint masks. To train it on synthetic data and then test it:
 

astrobee/localization/cnn_object_localization/tools/synthetic_segmentation_data/README.md

@@ -7,3 +7,11 @@
 5. Click the run button. Once all target objects are done, CTRL+C from the terminal to exit.
 
 NOTE: Due to a weird off-by-one bug that I can't quite eradicate, the first generated set of images (`$OUTPUT_DIR/colored_maps/colored_0000000.png`, `$OUTPUT_DIR/images/image_0000000.png`, and `$OUTPUT_DIR/labels_maps/labels_0000000.png`) do not have ground truth data recorded; nor do they get counted towards `NUM_IMAGES_EACH`. The easiest way to deal with this issue is to just delete these three images after data generation. `generate_data.sh` has an interrupt hook that should handle this, so you probably don't need to do anything assuming Ignition Gazebo quits gracefully (not a foregone conclusion), but be aware.
+
+## To run keypoint annotation (EXPERIMENTAL):
+
+- `./annotate_keypoints.sh -c CONFIG_NAME -o $OUTPUT_DIR`
+
+Note: If you're running the `../python_rcnn_training/src/*_kprcnn.py` scripts, you want to generate data using this tool.
+However, if you're working with the newer models in the `lightweight_cnn_object_localization` package, use the data generation tool there, as there have been some updates.
+

astrobee/localization/lightweight_cnn_object_localization/CMakeLists.txt

@@ -0,0 +1,3 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(lightweight_cnn_object_localization)
+add_subdirectory(tools/libtorch_frcnn_test/src)

astrobee/localization/lightweight_cnn_object_localization/README.md

@@ -0,0 +1,28 @@
+# Overview
+
+This module contains the code for handrail pose estimation. 
+The below documentation is in regards to running the handrail pose estimation nodes themselves (relevant code is in `launch`, `msg`, `nodes` and `src`.)
+The `tools` folder contains code for generating synthetic training data, as well as for training/testing the model. See the `README.md` files in that folder.
+Training data and pre-trained weights are not included in this git repository.
+
+# Running handrail ICP localization nodes in simulation
+
+## Preparation
+
+1. Set `dock_cam_rate = 1.0` in `astrobee/config/simulation/simulation.config`.
+2. Check that color dock camera data is being published. If necessary, modify the `image_topic` param value in `./launch/sim_handrail.launch` to point to this topic.
+3. Check that hardcoded camera parameters in `./src/cnn_object_localization/mrcnn_utils/undistorter.py` match those in `description/description/urdf/sensor_dock_cam.urdf.xacro`.
+4. Install prerequisites in `./requirements.txt`. (Do not use a venv; this will mess with ROS dependencies.)
+5. Ensure the reference pointcloud for a 30-inch handrail is available at `$CNN_OBJECT_LOCALIZATION_RESOURCES_PATH/reference_pointclouds/handrail_30.pcd`.
+6. Ensure the fine-tuned Mask-RCNN checkpoint is available at `$CNN_OBJECT_LOCALIZATION_RESOURCES_PATH/checkpoints/handrail_finetune_ckpt_199.pth`.
+
+## Running
+1. `source` the `devel/setup.bash` files for both Astrobee and ISSAC.
+2. Run `export CNN_OBJECT_LOCALIZATION_RESOURCES_PATH=...`
+3. Run `roslaunch astrobee sim.launch dds:=false robot:=sim_pub rviz:=true pose:="11.2573 -8.200 4.88988 0.00121545 -0.704632 0.00749165 0.709532"` (This launches the Classic Gazebo simulation with Astrobee initialized in a pose such that it is looking at a handrail.) 
+4. In another terminal, run `roslaunch ./launch/sim_handrail.launch`. (This launches the three nodes required for handrail segmentation, masking, and ICP.)
+
+# Running handrail ICP localization nodes on the Astrobee
+
+Not currently possible. The code currently has Python ROS nodes that can be installed on Astrobee, but the necessary Python dependencies cannot be installed. The segmentation model can be compiled to TorchScript, and there is a minimal pure C++ test for this model that can be run on Astrobee; however, the full handrail localization functionality of the Python ROS nodes has not been ported to Astrobee yet.
+

astrobee/localization/lightweight_cnn_object_localization/package.xml

@@ -0,0 +1,61 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>lightweight_cnn_object_localization</name>
+  <version>0.0.0</version>
+  <description>The lightweight_cnn_object_localization package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="[email protected]">Jane Doe</maintainer> -->
+  <maintainer email="[email protected]">anaveen</maintainer>
+
+
+  <!-- One license tag required, multiple allowed, one license per tag -->
+  <!-- Commonly used license strings: -->
+  <!--   BSD, MIT, Boost Software License, GPLv2, GPLv3, LGPLv2.1, LGPLv3 -->
+  <license>TODO</license>
+
+
+  <!-- Url tags are optional, but multiple are allowed, one per tag -->
+  <!-- Optional attribute type can be: website, bugtracker, or repository -->
+  <!-- Example: -->
+  <!-- <url type="website">http://wiki.ros.org/cnn_object_localization</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="[email protected]">Jane Doe</author> -->
+
+
+  <!-- The *depend tags are used to specify dependencies -->
+  <!-- Dependencies can be catkin packages or system dependencies -->
+  <!-- Examples: -->
+  <!-- Use depend as a shortcut for packages that are both build and exec dependencies -->
+  <!--   <depend>roscpp</depend> -->
+  <!--   Note that this is equivalent to the following: -->
+  <!--   <build_depend>roscpp</build_depend> -->
+  <!--   <exec_depend>roscpp</exec_depend> -->
+  <!-- Use build_depend for packages you need at compile time: -->
+  <!--   <build_depend>message_generation</build_depend> -->
+  <!-- Use build_export_depend for packages you need in order to build against this package: -->
+  <!--   <build_export_depend>message_generation</build_export_depend> -->
+  <!-- Use buildtool_depend for build tool packages: -->
+  <!--   <buildtool_depend>catkin</buildtool_depend> -->
+  <!-- Use exec_depend for packages you need at runtime: -->
+  <!--   <exec_depend>message_runtime</exec_depend> -->
+  <!-- Use test_depend for packages you need only for testing: -->
+  <!--   <test_depend>gtest</test_depend> -->
+  <!-- Use doc_depend for packages you need only for building documentation: -->
+  <!--   <doc_depend>doxygen</doc_depend> -->
+  <buildtool_depend>catkin</buildtool_depend>
+  <build_depend>message_generation</build_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

astrobee/localization/lightweight_cnn_object_localization/tools/README.md

@@ -0,0 +1,5 @@
+`synthetic_segmentation_data` contains code to generate synthetic training data for the handrail segmentation Mask-RCNN model using Ignition Gazebo.
+
+`pytorch_frcnn_training` contains code to train and test the Faster-RCNN model within Python, as well as to convert it to a TorchScript model.
+
+`libtorch_frcnn_test` contains code to test the TorchScript model within a C++ environment, as well as instructions on how to get `libtorch` and `libtorchvision` to work in a cross-compilation.

astrobee/localization/lightweight_cnn_object_localization/tools/libtorch_frcnn_test/.gitignore

@@ -0,0 +1,4 @@
+build
+libtorch
+vision
+*.zip

astrobee/localization/lightweight_cnn_object_localization/tools/libtorch_frcnn_test/README.md

@@ -0,0 +1,169 @@
+# Overview
+
+This tool is intended to test the TorchScript compiled model created using the `pytorch_frcnn_training` tool. Much more importantly, it provides documentation on how code that depends on `libtorch` and `libtorchvision` can be cross-compiled and installed on Astrobee. The below procedures are not particularly elegant or clean, but they work.
+
+# Building and running this tool locally
+
+- Make sure you are starting from scratch and don't have residue from previous installations of `libtorch` or `libtorchvision` lying around. This can cause mayhem, especially in the case of `libtorchvision`.
+- You will need `sudo` privileges.
+
+## Step 1: Installing libtorch
+
+### Option 1: Use a pre-built package
+
+- `cd <TORCH_INSTALL_DIR>` where `<TORCH_INSTALL_DIR>` is the directory you want `libtorch` to be downloaded and installed in.
+- `wget https://download.pytorch.org/libtorch/cpu/libtorch-cxx11-abi-shared-with-deps-1.13.1%2Bcpu.zip`
+- `unzip libtorch-cxx11-abi-shared-with-deps-1.13.1+cpu.zip; rm libtorch-cxx11-abi-shared-with-deps-1.13.1+cpu.zip`
+- `export CMAKE_PREFIX_PATH=<TORCH_INSTALL_DIR>/libtorch/share/cmake/Torch:${CMAKE_PREFIX_PATH}`. Consider adding this or some equivalent to `~/.bashrc`.
+
+### Option 2: Build from scratch
+
+- `cd <TORCH_INSTALL_DIR>` where `<TORCH_INSTALL_DIR>` is the directory you want `pytorch` to be downloaded, built, and installed in.
+- `git clone -b v1.13.1 --recurse-submodule https://github.com/pytorch/pytorch.git`
+- `cd pytorch; mkdir build_libtorch; cd build_libtorch`
+- `export USE_XNNPACK=0; export USE_CUDA=0; export USE_CUDNN=0; export USE_DISTRIBUTED=0; export USE_MKLDNN=0; export BUILD_TEST=0`.
+    - `USE_XNNPACK=0` because a bug in the Astrobee cross-compile toolchain prevents us from building it when cross-compiling, so we're setting it here too for consistency's sake.
+    - `USE_CUDA=0` and `USE_CUDNN=0` because the Astrobee doesn't have an NVidia GPU.
+    - `USE_DISTRIBUTED=0` because we aren't doing any distributed processing stuff.
+    - `USE_MKLDNN=0` because the Astrobee doesn't have an Intel CPU.
+    - `BUILD_TEST=0` to save time.
+- `python3 ../tools/build_libtorch.py`
+    - At this point, the script may complain about not having `typing_extensions`. Simply run `pip3 install typing_extensions` and try again.
+    - The script will automatically set the number of parallel jobs to the maximum. If your machine/VM starts struggling and/or you get an error `c++: fatal error: Killed signal terminated program cc1plus`, reduce the number of parallel jobs using `export MAX_JOBS=...` and then try again.
+- `export CMAKE_PREFIX_PATH=<TORCH_INSTALL_DIR>/pytorch/torch/share/cmake/Torch:${CMAKE_PREFIX_PATH}`. Consider adding this or some equivalent to `~/.bashrc`.
+
+## Step 2: Installing libtorchvision:
+
+- `cd <VISION_DOWNLOAD_DIR>` where `<VISION_DOWNLOAD_DIR>` is the directory you want `vision` to be downloaded and built, but not installed, in.
+- `git clone -b v0.14.1 --recurse-submodule https://github.com/pytorch/vision.git`
+- `cd vision; mkdir build; cd build`
+- `cmake ..`
+- `make`
+- `sudo make install`
+- `export CMAKE_PREFIX_PATH=<VISION_CONFIG_DIR>:${CMAKE_PREFIX_PATH}`, where `<VISION_CONFIG_DIR>` is the directory in which `TorchVisionConfig.cmake` was installed. This is probably something like `/usr/local/share/cmake/TorchVision`. (Consider adding this or some equivalent to `~/.bashrc`.)
+
+## Step 3: Building the test
+
+- `cd` back into this tool directory.
+- `cmake -S src -B build`
+- `cd build`
+- `make`
+
+## Step 4: Running the test
+
+- `./libtorch_frcnn_test <PATH/TO/TORCHSCRIPT/MODEL>`
+
+# Cross-compiling this tool and running on Astrobee
+
+- Make sure you are starting from scratch and don't have residue from previous installations of `libtorch` or `libtorchvision` lying around in your cross-compilation rootfs. This can cause mayhem, especially in the case of `libtorchvision`.
+- You will need `sudo` privileges.
+- To keep things simple, I choose to build everything in `${ARMHF_CHROOT_DIR}/root`. You can choose somewhere else.
+
+## Step 1: Getting ready
+
+- Ensure that your Astrobee install is set up for cross-compile.
+- Download or copy `chroot.sh` from https://babelfish.arc.nasa.gov/bitbucket/projects/ASTROBEE/repos/astrobee_platform/browse/rootfs/chroot.sh.
+- Make the following modifications (this makes something wonky but hopefully the subsequent steps still work):
+	- File: `chroot.sh`
+		- Lines: 34-38
+		    - Old: 
+                ```
+                chroot "$r" mount -t sysfs sysfs /sys
+                add_trap chroot "$r" umount /sys
+
+                chroot "$r" mount -t proc proc /proc
+                add_trap chroot "$r" umount /proc
+                ```
+		    - New:
+                ```
+                # chroot "$r" mount -t sysfs sysfs /sys
+                # add_trap chroot "$r" umount /sys
+
+                # chroot "$r" mount -t proc proc /proc
+                # add_trap chroot "$r" umount /proc
+                ```
+- `sudo su`
+- `./chroot.sh $ARMHF_CHROOT_DIR` (this will give you a shell inside the platform)
+
+## Step 2: Installing libtorch
+
+- Open a separate shell outside of the platform.
+
+### Step 2.1: typing_extensions
+
+- Outside the platform: `cd ${ARMHF_CHROOT_DIR}/root; pip3 install typing_extensions --target typing_extensions`
+- Inside the platform: `export PYTHONPATH=/root/typing_extensions:$PYTHONPATH`
+
+### Step 2.2: libtorch
+
+- Outside the platform: `cd ${ARMHF_CHROOT_DIR}/root; git clone -b v1.13.1 --recurse-submodule https://github.com/pytorch/pytorch.git`
+- Inside the platform: `cd /root/pytorch; mkdir build_libtorch; cd build_libtorch`
+- Inside the platform: `export USE_XNNPACK=0; export USE_CUDA=0; export USE_CUDNN=0; export USE_DISTRIBUTED=0; export USE_MKLDNN=0; export BUILD_TEST=0`
+    - `USE_XNNPACK=0` because a bug in the toolchain causes it to fail to build. [This bug is fixed in a more up-to-date version of gcc](https://gcc.gnu.org/bugzilla/show_bug.cgi?id=101723), but I don't want to poke around the toolchain stuff. If the version of gcc used ever gets upgraded, you can rebuild everything with XNNPACK and probably get a reasonable bump in model inference time in exchange for your efforts.
+    - `USE_CUDA=0` and `USE_CUDNN=0` because the Astrobee doesn't have an NVidia GPU.
+    - `USE_DISTRIBUTED=0` because we aren't doing any distributed processing stuff.
+    - `USE_MKLDNN=0` because the Astrobee doesn't have an Intel CPU.
+    - `BUILD_TEST=0` to save time.
+- Inside the platform: `python3 ../tools/build_libtorch.py`
+    - The script will attempt to set the number of parallel jobs to the maximum, which it erroneously believes to be 2. You can probably afford to speed up the build process by increasing the number of parallel jobs using `export MAX_JOBS=...`. If your machine/VM starts struggling and/or you get an error `c++: fatal error: Killed signal terminated program cc1plus`, you were too aggressive; back off and try again.
+
+## Step 3: Installing libtorchvision
+
+- Outside the platform: `cd ${ARMHF_CHROOT_DIR}/root`
+- Outside the platform: `git clone -b v0.14.1 --recurse-submodule https://github.com/pytorch/vision.git`
+- Inside the platform:  `cd /root/vision; mkdir build; cd build`
+- Inside the platform:  `export CMAKE_PREFIX_PATH=~/pytorch/torch/share/cmake/Torch:$CMAKE_PREFIX_PATH`
+- Inside the platform: `export USE_XNNPACK=0; export USE_CUDA=0; export USE_CUDNN=0; export USE_DISTRIBUTED=0; export USE_MKLDNN=0; export BUILD_TEST=0`
+- Inside the platform: `cmake ..`
+- Inside the platform: `make`
+- Inside the platform: `make install`
+
+## Step 4: Cross compiling Astrobee
+
+- This is just the normal Astrobee cross-compile process.
+- `cd $ASTROBEE_WS`
+- `./src/scripts/configure.sh -a; source ~/.bashrc; catkin build`
+
+## Step 5: Cross compiling ISAAC
+
+- Remove the `../../CATKIN_IGNORE` file. (It is there so that people uninterested in this functionality don't have to deal with Torch/TorchVision dependencies.)
+- Make the following modifications:
+	- File: `${ISAAC_WS}/src/scripts/configure.sh`
+		- Line: 306
+			- Old: 
+                ```
+                armhf_opts="-DCMAKE_TOOLCHAIN_FILE=${ff_path}/scripts/build/isaac_cross.cmake -DARMHF_ROS_DISTRO=${ros_version} -DCATKIN_ENABLE_TESTING=off
+                ```
+			- New: 
+                ```
+                armhf_opts="-DCMAKE_TOOLCHAIN_FILE=${ff_path}/scripts/build/isaac_cross.cmake -DARMHF_ROS_DISTRO=${ros_version} -DCATKIN_ENABLE_TESTING=off -DEXTRA_ROOT_PATH=${ARMHF_CHROOT_DIR}/root/pytorch/torch/share/cmake/Torch;${ARMHF_CHROOT_DIR}/usr/local/share/cmake/TorchVision"
+                ```
+		- Line: 322
+			- Old: 
+                ```
+                --whitelist isaac_astrobee_description isaac_util isaac_msgs inspection cargo isaac_hw_msgs wifi isaac gs_action_helper
+                ```
+			- New: 
+                ```
+                --whitelist isaac_astrobee_description isaac_util isaac_msgs inspection cargo isaac_hw_msgs wifi isaac gs_action_helper lightweight_cnn_object_localization
+                ```
+		- Line: 325
+			- Old: 
+                ```
+                --whitelist isaac_astrobee_description isaac_util isaac_msgs inspection cargo isaac_hw_msgs wifi isaac gs_action_helper
+                ```
+			- New: 
+                ```
+                --whitelist isaac_astrobee_description isaac_util isaac_msgs inspection cargo isaac_hw_msgs wifi isaac gs_action_helper lightweight_cnn_object_localization
+                ```
+- `cd $ISAAC_WS`
+- `./src/scripts/configure.sh -a; source ~/.bashrc; catkin build`
+
+## Step 6: Installing and running on Astrobee
+
+- `./scripts/prepare_shared_libraries --root=$ARMHF_CHROOT_DIR --output=$ISAAC_WS/armhf/opt/isaac/lib --libs=libc10.so,libtorchvision.so,libtorch_cpu.so,libtorch.so`
+    - This script finds the specified `.so` files in the root directory, then copies them to the output directory.
+- Follow the normal Astrobee installation procedure for ISAAC. The test executable should be located in `opt/isaac/bin`.
+- `scp` over your model weights.
+- On Astrobee: `export LD_LIBRARY_PATH=$LD_LIBRARY_PATH:/opt/isaac/lib`
+- On Astrobee: run the test executable.