Incorporating handrail segmentation into ISAAC

astrobee/localization/cnn_object_localization/CMakeLists.txt

@@ -0,0 +1,221 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(cnn_object_localization)
+
+## Compile as C++11, supported in ROS Kinetic and newer
+# add_compile_options(-std=c++11)
+
+## Find catkin macros and libraries
+## if COMPONENTS list like find_package(catkin REQUIRED COMPONENTS xyz)
+## is used, also find other catkin packages
+# Do not just add this to your CMakeLists.txt, modify the existing text to add message_generation before the closing parenthesis
+find_package(catkin REQUIRED COMPONENTS
+   roscpp
+   rospy
+   std_msgs
+   geometry_msgs
+   message_generation
+)
+
+## System dependencies are found with CMake's conventions
+# find_package(Boost REQUIRED COMPONENTS system)
+
+
+## Uncomment this if the package has a setup.py. This macro ensures
+## modules and global scripts declared therein get installed
+## See http://ros.org/doc/api/catkin/html/user_guide/setup_dot_py.html
+catkin_python_setup()
+
+################################################
+## Declare ROS messages, services and actions ##
+################################################
+
+## To declare and build messages, services or actions from within this
+## package, follow these steps:
+## * Let MSG_DEP_SET be the set of packages whose message types you use in
+##   your messages/services/actions (e.g. std_msgs, actionlib_msgs, ...).
+## * In the file package.xml:
+##   * add a build_depend tag for "message_generation"
+##   * add a build_depend and a exec_depend tag for each package in MSG_DEP_SET
+##   * If MSG_DEP_SET isn't empty the following dependency has been pulled in
+##     but can be declared for certainty nonetheless:
+##     * add a exec_depend tag for "message_runtime"
+## * In this file (CMakeLists.txt):
+##   * add "message_generation" and every package in MSG_DEP_SET to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * add "message_runtime" and every package in MSG_DEP_SET to
+##     catkin_package(CATKIN_DEPENDS ...)
+##   * uncomment the add_*_files sections below as needed
+##     and list every .msg/.srv/.action file to be processed
+##   * uncomment the generate_messages entry below
+##   * add every package in MSG_DEP_SET to generate_messages(DEPENDENCIES ...)
+
+## Generate messages in the 'msg' folder
+# add_message_files(
+#   FILES
+#   Message1.msg
+#   Message2.msg
+# )
+
+add_message_files(
+  FILES
+  EkfState.msg
+)
+
+## Generate services in the 'srv' folder
+# add_service_files(
+#   FILES
+#   Service1.srv
+#   Service2.srv
+# )
+
+## Generate actions in the 'action' folder
+# add_action_files(
+#   FILES
+#   Action1.action
+#   Action2.action
+# )
+
+## Generate added messages and services with any dependencies listed here
+generate_messages(
+   DEPENDENCIES
+   std_msgs  # Or other packages containing msgs
+   geometry_msgs
+)
+
+################################################
+## Declare ROS dynamic reconfigure parameters ##
+################################################
+
+## To declare and build dynamic reconfigure parameters within this
+## package, follow these steps:
+## * In the file package.xml:
+##   * add a build_depend and a exec_depend tag for "dynamic_reconfigure"
+## * In this file (CMakeLists.txt):
+##   * add "dynamic_reconfigure" to
+##     find_package(catkin REQUIRED COMPONENTS ...)
+##   * uncomment the "generate_dynamic_reconfigure_options" section below
+##     and list every .cfg file to be processed
+
+## Generate dynamic reconfigure parameters in the 'cfg' folder
+# generate_dynamic_reconfigure_options(
+#   cfg/DynReconf1.cfg
+#   cfg/DynReconf2.cfg
+# )
+
+###################################
+## catkin specific configuration ##
+###################################
+## The catkin_package macro generates cmake config files for your package
+## Declare things to be passed to dependent projects
+## INCLUDE_DIRS: uncomment this if your package contains header files
+## LIBRARIES: libraries you create in this project that dependent projects also need
+## CATKIN_DEPENDS: catkin_packages dependent projects also need
+## DEPENDS: system dependencies of this project that dependent projects also need
+catkin_package(
+#  INCLUDE_DIRS include
+#  LIBRARIES cnn_object_localization
+   CATKIN_DEPENDS message_runtime
+#  DEPENDS system_lib
+)
+
+###########
+## Build ##
+###########
+
+## Specify additional locations of header files
+## Your package locations should be listed before other locations
+# include_directories(
+# include
+# ${catkin_INCLUDE_DIRS}
+# )
+
+add_subdirectory(tools/libtorch_mrcnn_test)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/cnn_object_localization.cpp
+# )
+
+## Add cmake target dependencies of the library
+## as an example, code may need to be generated before libraries
+## either from message generation or dynamic reconfigure
+# add_dependencies(${PROJECT_NAME} ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Declare a C++ executable
+## With catkin_make all packages are built within a single CMake context
+## The recommended prefix ensures that target names across packages don't collide
+# add_executable(${PROJECT_NAME}_node src/cnn_object_localization_node.cpp)
+
+## Rename C++ executable without prefix
+## The above recommended prefix causes long target names, the following renames the
+## target back to the shorter version for ease of user use
+## e.g. "rosrun someones_pkg node" instead of "rosrun someones_pkg someones_pkg_node"
+# set_target_properties(${PROJECT_NAME}_node PROPERTIES OUTPUT_NAME node PREFIX "")
+
+## Add cmake target dependencies of the executable
+## same as for the library above
+# add_dependencies(${PROJECT_NAME}_node ${${PROJECT_NAME}_EXPORTED_TARGETS} ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(${PROJECT_NAME}_node
+#   ${catkin_LIBRARIES}
+# )
+
+#############
+## Install ##
+#############
+
+# all install targets should use catkin DESTINATION variables
+# See http://ros.org/doc/api/catkin/html/adv_user_guide/variables.html
+
+## Mark executable scripts (Python etc.) for installation
+## in contrast to setup.py, you can choose the destination
+catkin_install_python(PROGRAMS
+  nodes/astrobee_tf_broadcaster_node
+  nodes/handrail_detector_node
+  nodes/icp_pose_estimator_node
+  nodes/pointcloud_mask_node
+  nodes/visualize_detections_node
+  DESTINATION nodes
+)
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS ${PROJECT_NAME}_node
+#   RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark libraries for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_libraries.html
+# install(TARGETS ${PROJECT_NAME}
+#   ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION}
+#   RUNTIME DESTINATION ${CATKIN_GLOBAL_BIN_DESTINATION}
+# )
+
+## Mark cpp header files for installation
+# install(DIRECTORY include/${PROJECT_NAME}/
+#   DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION}
+#   FILES_MATCHING PATTERN "*.h"
+#   PATTERN ".svn" EXCLUDE
+# )
+
+## Mark other files for installation (e.g. launch and bag files, etc.)
+# install(FILES
+#   # myfile1
+#   # myfile2
+#   DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION}
+# )
+
+#############
+## Testing ##
+#############
+
+## Add gtest based cpp test target and link libraries
+# catkin_add_gtest(${PROJECT_NAME}-test test/test_cnn_object_localization.cpp)
+# if(TARGET ${PROJECT_NAME}-test)
+#   target_link_libraries(${PROJECT_NAME}-test ${PROJECT_NAME})
+# endif()
+
+## Add folders to be run by python nosetests
+# catkin_add_nosetests(test)

astrobee/localization/cnn_object_localization/README.md

@@ -0,0 +1,47 @@
+# 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. 
+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/cnn_object_localization/launch/real_handrail.launch

@@ -0,0 +1,19 @@
+<launch>
+  <node pkg="rosbag" type="play" name="player" output="screen" args="--loop --clock --quiet /path/to/data.bag">
+    <param name="/use_sim_time" value="true" />
+  </node>
+  <!-- <node pkg= "cnn_object_localization" type="astrobee_tf_broadcaster_node" name="astrobee_tf_broadcaster_node" output= "screen">
+    <param name="/use_sim_time" value="true" />
+  </node> -->
+  <node pkg= "cnn_object_localization" type="handrail_detector_node" name="handrail_detector_node" output= "screen">
+      <param name="image_topic" value="/hw/cam_dock_color" />
+      <param name="/use_sim_time" value="true" />
+  </node>
+  <node pkg= "cnn_object_localization" type="pointcloud_mask_node" name="pointcloud_mask_processor_node" output= "screen">
+      <param name="rgb_image_topic"  value="/hw/cam_dock_color" />
+      <param name="/use_sim_time" value="true" />
+  </node>
+  <node pkg= "cnn_object_localization" type="icp_pose_estimator_node" name="icp_pose_estimator_node" output= "screen">
+    <param name="/use_sim_time" value="true" />
+  </node>
+</launch>

astrobee/localization/cnn_object_localization/launch/sim_handrail.launch

@@ -0,0 +1,9 @@
+<launch>
+  <node pkg= "cnn_object_localization" type="handrail_detector_node" name="handrail_detector_node" output= "screen">
+      <param name="image_topic" value="/hw/cam_dock" />
+  </node>
+  <node pkg= "cnn_object_localization" type="pointcloud_mask_node" name="pointcloud_mask_processor_node" output= "screen">
+      <param name="rgb_image_topic"  value="/hw/cam_dock" />
+  </node>
+  <node pkg= "cnn_object_localization" type="icp_pose_estimator_node" name="icp_pose_estimator_node" output= "screen"/>
+</launch>

astrobee/localization/cnn_object_localization/msg/EkfState.msg

@@ -0,0 +1,64 @@
+# 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.
+#
+# An observation of a handrail from a depth image.
+
+std_msgs/Header header # header with timestamp
+string child_frame_id # frame ID
+
+geometry_msgs/Pose pose # robot body pose
+
+# m/s
+geometry_msgs/Vector3 velocity # the body velocity
+
+# rad/s
+geometry_msgs/Vector3 omega # body rotational velocity
+geometry_msgs/Vector3 gyro_bias # estimated gyro bias
+
+# m/s/s
+geometry_msgs/Vector3 accel # acceleration in body frame
+geometry_msgs/Vector3 accel_bias # estimated accel bias
+
+# Filter Health
+
+# covariance diagonal. 1-3 orientation, 4-6 gyro bias, 7-9 velocity, 10-12 accel bias, 13-15 position
+float32[15] cov_diag
+# confidence in EKF. 0 is good, 1 is a bit confused, 2 is lost
+uint8 confidence
+uint8 CONFIDENCE_GOOD = 0	# Tracking well
+uint8 CONFIDENCE_POOR = 1	# Tracking poorly
+uint8 CONFIDENCE_LOST = 2	# We are lost
+
+uint8 aug_state_enum # bitmask of augmented states intialized
+
+# status byte sent by GNC
+uint8 status
+uint8 STATUS_INVALID = 255	# invalid
+
+# optical flow features this frame (0 if no update)
+uint8 of_count
+# ml features this frame (0 if no update)
+uint8 ml_count
+
+# Global Handrail Pose
+geometry_msgs/Pose hr_global_pose
+
+# mahalanobis distances for features
+float32[50] ml_mahal_dists
+
+# Are we busy estimating the bias?
+bool estimating_bias

astrobee/localization/cnn_object_localization/nodes/astrobee_tf_broadcaster_node

@@ -0,0 +1,8 @@
+#!/usr/bin/env python3
+from cnn_object_localization.astrobee_tf_broadcaster import *
+
+
+if __name__ == "__main__":
+    rospy.init_node("astrobee_tf_broadcaster_node")
+    rospy.Subscriber("/gnc/ekf", EkfState, astrobee_pose_callback, buff_size=2**24)
+    rospy.spin()

astrobee/localization/cnn_object_localization/nodes/handrail_detector_node

@@ -0,0 +1,12 @@
+#!/usr/bin/env python3
+from cnn_object_localization.handrail_detector import *
+
+if __name__ == "__main__":
+    # Initialize node
+    rospy.init_node("handrail_detector_node")
+
+    # Define detector object
+    dm = HandrailDetectorManager()
+
+    # Spin
+    rospy.spin()