Merge ad673ada4a into b1ed863973

.gitignore

@@ -1,3 +1,5 @@
+**/.vscode
+
 # Emacs related
 *~
 \#*\#
@@ -17,3 +19,4 @@ stretch_description/urdf/exported_urdf/*
 stretch_description/urdf/exported_urdf_previous
 stretch_description/urdf/exported_urdf_previous/*
 stretch_funmap/src/stretch_funmap/cython_min_cost_path.c
+vz_ros_wrappers/bags/

stretch_deep_perception/nodes/NCS2_guide/NCS2_guide_detect_faces.py

@@ -0,0 +1,86 @@
+#!/usr/bin/env python3
+
+import cv2
+import sys
+import rospy
+import head_estimator as he
+import detection_node as dn
+import deep_learning_model_options as do
+
+
+if __name__ == '__main__':    
+    print('cv2.__version__ =', cv2.__version__)
+    print('Python version (must be > 3.0):', sys.version)
+    assert(int(sys.version[0]) >= 3)
+    
+    ##############################################
+    # Perform coarse filtering of 3D points using anthropometry
+    #
+    # 30cm should be significantly over the maximum dimensions of a human head
+    # 10cm should be significantly smaller than the dimensions of an adult head
+    # https://en.wikipedia.org/wiki/Human_head
+
+    # children "attain 30% of adult head width by the middle of
+    # prenatal life, 60% by birth, 80% by 6 postnatal months, 90%
+    # by 3 years and 95% by 9 years" - GROWTH IN HEAD WIDTH DURING
+    # THE FIRST TWELVE YEARS OF LIFE HOWARD V. MEREDITH, copyright
+    # 1953 by the American Academy of Pediatrics
+    # https://pediatrics.aappublications.org/content/12/4/411
+
+    # Filtering for depths corresponding with heads with heights
+    # or widths from 8cm to 40cm should be conservative.
+    min_head_m = 0.08
+    max_head_m = 0.4
+    ##############################################
+
+    #### load model directory where all the deep perception models are stored   
+    models_directory = do.get_directory()
+    print('Using the following directory for deep learning models:', models_directory) 
+    
+    ######################################################
+    # check if the neural compute stick is able to run the deep learning model. 
+    # Not all models are compatile to run on NCS2.
+    # Currently this only work for two models: object and face
+    # detection.  Currently it does not work with head pose
+    # estimation, facial landmarks, and body landmarks.
+    # The reason is that NCS only supports models to be run through OpenVINO. 
+    # There are OpenVINO format model zoos availble on the above mentioned models.
+    # In case you want to run a model which is not availble in the model zoo, 
+    # There is an API availabe to convert any format into OpenVINO format.
+    # https://docs.openvino.ai/2021.2/openvino_docs_MO_DG_prepare_model_convert_model_Convert_Model_From_Caffe.html
+
+    # The neural compute stick is called in deep_learning_model_options Refer that file for further instructions
+    
+    #####################################################
+
+
+    use_neural_compute_stick = do.use_neural_compute_stick()
+    if use_neural_compute_stick:
+        print('Attempt to use an Intel Neural Compute Stick 2.')
+    else:
+        print('Not attempting to use an Intel Neural Compute Stick 2.')
+    
+    #  Each model has to have two formats one to run on nomal cpu which can be the direct caffe/pytorch/tensorflow etc., format
+    # second is the OpenVINO format.
+    # If the model is compatible to run on NCS 2, make sure to turn on the NCS feature by passing the use_neural_compute_stick.
+    detector = he.HeadPoseEstimator(models_directory,
+                                    use_neural_compute_stick=use_neural_compute_stick)
+
+    ### The rest of the code is normal
+    default_marker_name = 'face'
+    node_name = 'DetectFacesNode'
+    topic_base_name = 'faces'
+    fit_plane = False
+    node = dn.DetectionNode(detector,
+                            default_marker_name,
+                            node_name,
+                            topic_base_name,
+                            fit_plane,
+                            min_box_side_m=min_head_m,
+                            max_box_side_m=max_head_m)
+    node.main()
+    try:
+        rospy.spin()
+    except KeyboardInterrupt:
+        print('interrupt received, so shutting down')
+

stretch_deep_perception/nodes/NCS2_guide/NCS2_guide_head_estimator.py

@@ -0,0 +1,322 @@
+#!/usr/bin/env python3
+
+import cv2
+import numpy as np
+from scipy.spatial.transform import Rotation
+import deep_models_shared as dm
+
+class HeadPoseEstimator:
+    def __init__(self, models_directory, use_neural_compute_stick=False):
+        # Load the models to run on CPU
+        #########################################################################
+        
+        # Specify model directory and load caffe / prototxt models 
+        # Load weights and config files from this directory 
+        # Note: These models will not run on compute stick
+        #########################################################################
+        models_dir = models_directory
+        print('Using the following directory to load object detector models:', models_dir)
+        
+        # file with network architecture and other information
+        head_detection_model_prototxt_filename = models_dir + '/head_detection/deploy.prototxt'
+        # file with network weights
+        head_detection_model_caffemodel_filename = models_dir + '/head_detection/res10_300x300_ssd_iter_140000.caffemodel'
+        self.face_confidence_threshold = 0.2
+
+        print('attempting to load neural network from files')
+        print('prototxt file =', head_detection_model_prototxt_filename)
+        print('caffemodel file =', head_detection_model_caffemodel_filename)
+        self.head_detection_model = cv2.dnn.readNetFromCaffe(head_detection_model_prototxt_filename, head_detection_model_caffemodel_filename)
+        dm.print_model_info(self.head_detection_model, 'head_detection_model')
+        
+        # Load the models to run on VPU (NCS 2)
+
+        #########################################################################
+        # If neural compute stick is available then run the models on Myriad using the command setPreferableTarget
+        # Load model directory from the OpenVINO model zoo 
+        # Load weights and config files from this directory 
+        #
+        #########################################################################
+        if use_neural_compute_stick:
+            print('HeadPoseEstimator.__init__: Attempting to use an Intel Neural Compute Stick 2 using the following command: self.head_detection_model.setPreferableTarget(cv2.dnn.DNN_TARGET_MYRIAD)')
+            self.head_detection_model.setPreferableTarget(cv2.dnn.DNN_TARGET_MYRIAD)
+
+        head_pose_model_dir = models_dir + '/open_model_zoo/head-pose-estimation-adas-0001/FP32/'
+        head_pose_weights_filename = head_pose_model_dir + 'head-pose-estimation-adas-0001.bin'
+        head_pose_config_filename = head_pose_model_dir + 'head-pose-estimation-adas-0001.xml'
+        self.head_pose_model = cv2.dnn.readNet(head_pose_weights_filename, head_pose_config_filename)
+        
+        if use_neural_compute_stick:
+            print('Not attempting to use a Intel Neural Compute Stick 2 for head pose estimation due to potential errors.')
+
+        dm.print_model_info(self.head_pose_model, 'head_pose_model')
+
+        landmarks_model_dir = models_dir + '/open_model_zoo/facial-landmarks-35-adas-0002/FP32/'
+        landmarks_weights_filename = landmarks_model_dir + 'facial-landmarks-35-adas-0002.bin'
+        landmarks_config_filename = landmarks_model_dir + 'facial-landmarks-35-adas-0002.xml'
+        self.landmarks_model = cv2.dnn.readNet(landmarks_weights_filename, landmarks_config_filename)
+
+        if use_neural_compute_stick:
+            print('Not attempting to use a Intel Neural Compute Stick 2 for facial landmarks due to potential errors.')
+
+        ### The rest of the code is as is
+        dm.print_model_info(self.head_pose_model, 'head_pose_model')
+
+        
+        dm.print_model_info(self.landmarks_model, 'landmarks_model')
+        
+        self.landmark_names = ['right_eye_left', 'right_eye_right',
+                               'left_eye_right', 'left_eye_left', 'nose_tip',
+                               'nose_bottom', 'nose_right', 'nose_left', 'mouth_right',
+                               'mouth_left', 'mouth_top', 'mouth_bottom',
+                               'right_eyebrow_right', 'right_eyebrow_middle', 'right_eyebrow_left',
+                               'left_eyebrow_right', 'left_eyebrow_middle', 'left_eyebrow_left',
+                               'right_cheek_18', 'right_cheek_19', 'right_cheek_20', 'right_cheek_21',
+                               'right_cheek_22', 'right_cheek_23', 'right_cheek_24',
+                               'chin_right', 'chin_middle', 'chin_left',
+                               'left_cheek_28', 'left_cheek_29', 'left_cheek_30', 'left_cheek_31',
+                               'left_cheek_32', 'left_cheek_33', 'left_cheek_34']
+    
+
+    def get_landmark_names(self):
+        return self.landmark_names
+
+    def get_landmark_colors(self):
+        return None
+
+    def get_landmark_color_dict(self):
+        return None
+
+    def detect_faces(self, rgb_image):
+        orig_h, orig_w, c = rgb_image.shape
+        face_image = rgb_image
+        rot_h, rot_w, c = face_image.shape
+        # Assumes that the width is smaller than the height, and crop
+        # a width x width square image from the top.
+        square_face_image = face_image[:rot_w, :, :]
+        sqr_h, sqr_w, c = square_face_image.shape
+        network_image = cv2.resize(square_face_image, (300, 300))
+        # Some magic numbers came from
+        # https://www.pyimagesearch.com/2018/02/26/face-detection-with-opencv-and-deep-learning/
+        face_image_blob = cv2.dnn.blobFromImage(network_image, 1.0, (300, 300), (104.0, 177.0, 123.0))
+        self.head_detection_model.setInput(face_image_blob)
+        face_detections = self.head_detection_model.forward()[0,0,:,:]
+        confidence_mask = face_detections[:, 2] > self.face_confidence_threshold
+        face_detections = face_detections[confidence_mask]
+        coordinates = face_detections[:, 3:7]
+        # Scale and rotate coordinates to the original image
+        coordinates = coordinates * np.array([sqr_w, sqr_h, sqr_w, sqr_h])
+
+        face_id = 0
+        boxes = []
+
+        for x0, y0, x1, y1 in coordinates:
+            orig_y0 = y0
+            orig_y1 = y1
+            orig_x0 = x0
+            orig_x1 = x1
+            face_id += 1
+            bounding_box = [orig_x0, orig_y0, orig_x1, orig_y1]
+            boxes.append(bounding_box)
+
+        return boxes
+
+    
+    def get_sub_image(self, rgb_image, bounding_box, enlarge_box=True, enlarge_scale=1.15):
+        if enlarge_box:
+            scale = enlarge_scale
+            orig_h, orig_w, c = rgb_image.shape
+
+            x0 = bounding_box[0]
+            y0 = bounding_box[1]
+            x1 = bounding_box[2]
+            y1 = bounding_box[3]
+
+            m_x = (x1 + x0) / 2.0
+            m_y = (y1 + y0) / 2.0
+
+            b_w = x1 - x0
+            b_h = y1 - y0
+
+            b_w = scale * b_w
+            b_h = scale * b_h
+
+            x0 = int(round(m_x - (b_w/2.0)))
+            x1 = int(round(m_x + (b_w/2.0)))
+            y0 = int(round(m_y - (b_h/2.0)))
+            y1 = int(round(m_y + (b_h/2.0)))
+
+            x0 = max(0, x0)
+            x1 = min(orig_w, x1)
+            y0 = max(0, y0)
+            y1 = min(orig_h, y1)
+        else: 
+            x0 = int(round(bounding_box[0]))
+            y0 = int(round(bounding_box[1]))
+            x1 = int(round(bounding_box[2]))
+            y1 = int(round(bounding_box[3]))
+
+        actual_bounding_box = [x0, y0, x1, y1]
+        image_to_crop = rgb_image
+        sub_image = image_to_crop[y0:y1, x0:x1, :]
+        return sub_image, actual_bounding_box
+    
+
+    def estimate_head_pose(self, rgb_image, bounding_box, enlarge_box=True, enlarge_scale=1.15):
+        face_crop_image, actual_bounding_box = self.get_sub_image(rgb_image, bounding_box, enlarge_box=enlarge_box, enlarge_scale=enlarge_scale)
+        sqr_h, sqr_w, c = face_crop_image.shape
+
+        if (sqr_h > 0) and (sqr_w > 0):
+            head_pose_image_blob = cv2.dnn.blobFromImage(face_crop_image,
+                                                         size=(60, 60),
+                                                         swapRB=False,
+                                                         crop=False,
+                                                         ddepth=cv2.CV_32F)
+            self.head_pose_model.setInput(head_pose_image_blob)
+            head_pose_out = self.head_pose_model.forward(['angle_r_fc', 'angle_p_fc', 'angle_y_fc'])
+            rpy = head_pose_out
+            roll = rpy[0][0][0]
+            pitch = rpy[1][0][0]
+            yaw = rpy[2][0][0]
+            pitch = pitch * np.pi/180.0
+            roll = roll * np.pi/180.0
+            yaw = yaw * np.pi/180.0
+            
+            return yaw, pitch, roll
+
+        return None, None, None
+
+
+    def detect_facial_landmarks(self, rgb_image, bounding_box, enlarge_box=True, enlarge_scale=1.15):
+        face_crop_image, actual_bounding_box = self.get_sub_image(rgb_image, bounding_box, enlarge_box=enlarge_box, enlarge_scale=enlarge_scale)
+        sqr_h, sqr_w, c = face_crop_image.shape
+
+        if (sqr_h > 0) and (sqr_w > 0):
+            landmarks_image_blob = cv2.dnn.blobFromImage(face_crop_image,
+                                                         size=(60, 60),
+                                                         swapRB=False,
+                                                         crop=False,
+                                                         ddepth=cv2.CV_32F)
+            self.landmarks_model.setInput(landmarks_image_blob)
+            landmarks_out = self.landmarks_model.forward()
+
+            s = landmarks_out.shape
+            out = np.reshape(landmarks_out[0], (s[1]//2, 2))
+            x0, y0, x1, y1 = actual_bounding_box
+
+            landmarks = {}
+            for n, v in enumerate(out):
+                x = int(round((v[0] * sqr_w) + x0))
+                y = int(round((v[1] * sqr_h) + y0))
+                name = self.landmark_names[n]
+                landmarks[name] = (x,y)
+
+            return landmarks, self.landmark_names.copy()
+        return None, None
+
+
+    def draw_bounding_box(self, image, bounding_box):
+        x0 = int(round(bounding_box[0]))
+        y0 = int(round(bounding_box[1]))
+        x1 = int(round(bounding_box[2]))
+        y1 = int(round(bounding_box[3]))
+        color = (0, 0, 255)
+        thickness = 2
+        cv2.rectangle(image, (x0, y0), (x1, y1), color, thickness)
+
+        
+    def draw_head_pose(self, image, yaw, pitch, roll, bounding_box):
+        x0, y0, x1, y1 = bounding_box
+        face_x = (x1 + x0) / 2.0
+        face_y = (y1 + y0) / 2.0
+        #
+        # opencv uses right-handed coordinate system
+        # x points to the right of the image
+        # y points to the bottom of the image
+        # z points into the image
+        #
+
+        h, w, c = image.shape
+        camera_center = (w/2.0, h/2.0)
+        #For rendering with an unknown camera
+        focal_length = 50.0 
+        camera_matrix = np.array([[focal_length, 0.0,          camera_center[0]],
+                                  [0.0,          focal_length, camera_center[1]],
+                                  [0.0,          0.0,          1.0]])
+        face_translation = np.array([0.0, 0.0, 3000.0])
+        distortion_coefficients = np.array([0.0, 0.0, 0.0, 0.0])
+        # negate the directions of the y and z axes
+        axes = np.array([[2000.0,  0.0,      0.0   ],
+                         [0.0,     -2000.0,  0.0   ],
+                         [0.0,     0.0,      -2000.0],
+                         [0.0,     0.0,      0.0   ]])
+        head_ypr = np.array([-yaw, pitch, roll])
+        rotation_mat = Rotation.from_euler('yxz', head_ypr).as_matrix()
+        rotation_vec, jacobian = cv2.Rodrigues(rotation_mat)
+        image_points, jacobian = cv2.projectPoints(axes, rotation_vec, face_translation, camera_matrix, distortion_coefficients)
+        face_pix = np.array([face_x, face_y])
+
+        origin = image_points[3].ravel()
+        x_axis = (image_points[0].ravel() - origin) + face_pix
+        y_axis = (image_points[1].ravel() - origin) + face_pix
+        z_axis = (image_points[2].ravel() - origin) + face_pix
+
+        p0 = tuple(np.int32(np.round(face_pix)))
+        p1 = tuple(np.int32(np.round(x_axis)))
+        cv2.line(image, p0, p1, (0, 0, 255), 2)
+        p1 = tuple(np.int32(np.round(y_axis)))
+        cv2.line(image, p0, p1, (0, 255, 0), 2)
+        p1 = tuple(np.int32(np.round(z_axis)))
+        cv2.line(image, p0, p1, (255, 0, 0), 2)
+
+        
+    def draw_landmarks(self, image, landmarks):
+        for name, xy in landmarks.items():
+            x = xy[0]
+            y = xy[1]
+            if 'mouth' in name:
+                color = (255, 0, 0)
+            elif 'nose' in name:
+                color = (0, 255, 0)
+            elif 'eyebrow' in name:
+                color = (0, 0, 0)
+            elif 'right_eye' in name:
+                color = (255, 255, 0)
+            elif 'left_eye' in name:
+                color = (0, 255, 255)
+            elif 'chin' in name:
+                color = (255, 0, 255)
+            else:
+                color = (0, 0, 255)
+            cv2.circle(image, (x,y), 2, color, 1)
+            font_scale = 1.0
+            line_color = [0, 0, 0]
+            line_width = 1
+            font = cv2.FONT_HERSHEY_PLAIN 
+
+            
+    def apply_to_image(self, rgb_image, draw_output=False):
+        if draw_output:
+            output_image = rgb_image.copy()
+        else:
+            output_image = None
+
+        heads = []
+        boxes = self.detect_faces(rgb_image)
+        facial_landmark_names = self.landmark_names.copy()
+        for bounding_box in boxes:
+            if draw_output: 
+                self.draw_bounding_box(output_image, bounding_box)
+            yaw, pitch, roll = self.estimate_head_pose(rgb_image, bounding_box, enlarge_box=True, enlarge_scale=1.15)
+            if yaw is not None: 
+                ypr = (yaw, pitch, roll)
+                if draw_output: 
+                    self.draw_head_pose(output_image, yaw, pitch, roll, bounding_box)
+            else:
+                ypr = None
+            landmarks, landmark_names = self.detect_facial_landmarks(rgb_image, bounding_box, enlarge_box=True, enlarge_scale=1.15)
+            if (landmarks is not None) and draw_output: 
+                self.draw_landmarks(output_image, landmarks)
+            heads.append({'box':bounding_box, 'ypr':ypr, 'landmarks':landmarks})
+
+        return heads, output_image

stretch_gazebo/maps/willowgarage.yaml

@@ -0,0 +1,7 @@
+image: /home/ananya/Code/VZ_Project/Robot_Autonomous_Navigation/catkin_ws/src/stretch_ros/stretch_gazebo/maps/willowgarage.pgm
+resolution: 0.050000
+origin: [-100.000000, -100.000000, 0.000000]
+negate: 0
+occupied_thresh: 0.65
+free_thresh: 0.196
+

stretch_gazebo/worlds/willowgarageempty.world

@@ -0,0 +1,14 @@
+<?xml version="1.0" ?>
+<sdf version="1.5">
+  <world name="default">
+    <include>
+      <uri>model://ground_plane</uri>
+    </include>
+    <include>
+      <uri>model://sun</uri>
+    </include>
+    <include>
+      <uri>model://willowgarage</uri>
+    </include>
+  </world>
+</sdf>

vz_acoustic_scene_analysis/CMakeLists.txt

@@ -0,0 +1,220 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(vz_acoustic_scene_analysis)
+
+## 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
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  # opencv2 # ananya this does not compile
+  roscpp
+  rospy
+  audio_common_msgs
+  sensor_msgs
+  std_msgs
+  message_generation
+)
+
+find_package(PkgConfig)
+pkg_check_modules(GST1.0 gstreamer-1.0 REQUIRED)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS thread)
+
+
+## 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
+  VZ_AudioInfo.msg
+  VZ_AudioData.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
+)
+
+################################################
+## 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 vz_acoustic_scene_analysis
+  CATKIN_DEPENDS roscpp rospy sensor_msgs std_msgs 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}
+  ${Boost_INCLUDE_DIRS}
+  ${GST1.0_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/vz_ros_wrappers.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(audio_capture scripts/audio_capture.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(audio_capture ${catkin_EXPORTED_TARGETS})
+
+## Specify libraries to link a library or executable target against
+# target_link_libraries(audio_capture
+#    ${catkin_LIBRARIES}
+#    ${GST1.0_LIBRARIES}
+#    ${Boost_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
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+# install(TARGETS audio_capture
+#   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 launch
+   DESTINATION ${CATKIN_PACKAGE_SHARE_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_vz_ros_wrappers.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)

vz_acoustic_scene_analysis/config/audio_params.yaml

@@ -0,0 +1,10 @@
+TIMEOUT_MILLISECONDS: 100000
+VENDOR_ID: 0x2886
+PRODUCT_ID: 0x0018
+RESPEAKER_RATE: 16000
+RESPEAKER_CHANNELS: 6 # must flash 6_channels_firmware.bin first
+RESPEAKER_WIDTH: 2
+RESPEAKER_INDEX: get_respeaker_device_id()
+CHUNK: 1024
+CHUNK_SIZE: 0.5
+SECONDS: 5

vz_acoustic_scene_analysis/launch/wav_audio.launch

@@ -0,0 +1,4 @@
+<launch>
+    <rosparam ns="VZ_ACOUSTIC_SCENE_ANALYSIS" command="load" file="$(find vz_acoustic_scene_analysis)/config/audio_params.yaml" />
+    <node name="stretch_audio" pkg="vz_acoustic_scene_analysis" type="ros_interface.py" output="screen" />
+  </launch>

vz_acoustic_scene_analysis/msg/VZ_AudioData.msg

@@ -0,0 +1 @@
+int16[] data # Represents raw wav audio data

vz_acoustic_scene_analysis/msg/VZ_AudioInfo.msg

@@ -0,0 +1,12 @@
+# This message contains the audio meta data
+
+# Number of channels
+uint8 channels
+# Sampling rate [Hz]
+uint32 sample_rate
+# Audio format (e.g. S16LE)
+string sample_format
+# Amount of audio data per second [bits/s]
+uint32 bitrate
+# Audio coding format (e.g. WAVE, MP3)
+string coding_format

vz_acoustic_scene_analysis/package.xml

@@ -0,0 +1,85 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vz_acoustic_scene_analysis</name>
+  <version>0.0.0</version>
+  <description>The vz_acoustic_scene_analysis package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ananya@todo.todo">ananya</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/vz_acoustic_scene_analysis</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">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>cv_bridge</build_depend>
+  <!-- <build_depend>opencv2</build_depend> -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>audio_common_msgs</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>libgstreamer1.0-dev</build_depend>
+  <build_depend>libgstreamer-plugins-base1.0-dev</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_depend>message_generation</build_depend>
+  <!-- <build_export_depend>opencv2</build_export_depend> -->
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <!-- <exec_depend>opencv2</exec_depend> -->
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>message_runtime</exec_depend>
+
+  <exec_depend>audio_common_msgs</exec_depend>
+  <exec_depend>gstreamer1.0</exec_depend>
+  <exec_depend>gstreamer1.0-plugins-base</exec_depend>
+  <exec_depend>gstreamer1.0-plugins-good</exec_depend>
+  <exec_depend>gstreamer1.0-plugins-ugly</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

vz_acoustic_scene_analysis/scripts/ros_interface.py

@@ -0,0 +1,316 @@
+#!/usr/bin/env python3
+
+# Non ROS imports
+from __future__ import print_function
+import pyaudio
+import numpy as np
+import usb.core
+import struct
+import scipy
+import sys
+import os
+import os.path
+import pickle
+from contextlib import contextmanager
+
+# ROS specific imports 
+import rospy
+from rospy.numpy_msg import numpy_msg
+
+# Custom imports
+import stretch_body.hello_utils as hu
+hu.print_stretch_re_use()
+from vz_acoustic_scene_analysis.msg import VZ_AudioData
+import NEU_VZ_ASA.MehrshadTesting.codes.A_CoughDetection.src.DSP as dsp
+
+
+@contextmanager
+def ignore_stderr():
+    devnull = None
+    try:
+        devnull = os.open(os.devnull, os.O_WRONLY)
+        stderr = os.dup(2)
+        sys.stderr.flush()
+        os.dup2(devnull, 2)
+        try:
+            yield
+        finally:
+            os.dup2(stderr, 2)
+            os.close(stderr)
+    finally:
+        if devnull is not None:
+            os.close(devnull)
+
+
+# parameter list
+# name: (id, offset, type, max, min , r/w, info)
+PARAMETERS = {
+    'AECFREEZEONOFF': (18, 7, 'int', 1, 0, 'rw', 'Adaptive Echo Canceler updates inhibit.', '0 = Adaptation enabled', '1 = Freeze adaptation, filter only'),
+    'AECNORM': (18, 19, 'float', 16, 0.25, 'rw', 'Limit on norm of AEC filter coefficients'),
+    'AECPATHCHANGE': (18, 25, 'int', 1, 0, 'ro', 'AEC Path Change Detection.', '0 = false (no path change detected)', '1 = true (path change detected)'),
+    'RT60': (18, 26, 'float', 0.9, 0.25, 'ro', 'Current RT60 estimate in seconds'),
+    'HPFONOFF': (18, 27, 'int', 3, 0, 'rw', 'High-pass Filter on microphone signals.', '0 = OFF', '1 = ON - 70 Hz cut-off', '2 = ON - 125 Hz cut-off', '3 = ON - 180 Hz cut-off'),
+    'RT60ONOFF': (18, 28, 'int', 1, 0, 'rw', 'RT60 Estimation for AES. 0 = OFF 1 = ON'),
+    'AECSILENCELEVEL': (18, 30, 'float', 1, 1e-09, 'rw', 'Threshold for signal detection in AEC [-inf .. 0] dBov (Default: -80dBov = 10log10(1x10-8))'),
+    'AECSILENCEMODE': (18, 31, 'int', 1, 0, 'ro', 'AEC far-end silence detection status. ', '0 = false (signal detected) ', '1 = true (silence detected)'),
+    'AGCONOFF': (19, 0, 'int', 1, 0, 'rw', 'Automatic Gain Control. ', '0 = OFF ', '1 = ON'),
+    'AGCMAXGAIN': (19, 1, 'float', 1000, 1, 'rw', 'Maximum AGC gain factor. ', '[0 .. 60] dB (default 30dB = 20log10(31.6))'),
+    'AGCDESIREDLEVEL': (19, 2, 'float', 0.99, 1e-08, 'rw', 'Target power level of the output signal. ', '[-inf .. 0] dBov (default: -23dBov = 10log10(0.005))'),
+    'AGCGAIN': (19, 3, 'float', 1000, 1, 'rw', 'Current AGC gain factor. ', '[0 .. 60] dB (default: 0.0dB = 20log10(1.0))'),
+    'AGCTIME': (19, 4, 'float', 1, 0.1, 'rw', 'Ramps-up / down time-constant in seconds.'),
+    'CNIONOFF': (19, 5, 'int', 1, 0, 'rw', 'Comfort Noise Insertion.', '0 = OFF', '1 = ON'),
+    'FREEZEONOFF': (19, 6, 'int', 1, 0, 'rw', 'Adaptive beamformer updates.', '0 = Adaptation enabled', '1 = Freeze adaptation, filter only'),
+    'STATNOISEONOFF': (19, 8, 'int', 1, 0, 'rw', 'Stationary noise suppression.', '0 = OFF', '1 = ON'),
+    'GAMMA_NS': (19, 9, 'float', 3, 0, 'rw', 'Over-subtraction factor of stationary noise. min .. max attenuation'),
+    'MIN_NS': (19, 10, 'float', 1, 0, 'rw', 'Gain-floor for stationary noise suppression.', '[-inf .. 0] dB (default: -16dB = 20log10(0.15))'),
+    'NONSTATNOISEONOFF': (19, 11, 'int', 1, 0, 'rw', 'Non-stationary noise suppression.', '0 = OFF', '1 = ON'),
+    'GAMMA_NN': (19, 12, 'float', 3, 0, 'rw', 'Over-subtraction factor of non- stationary noise. min .. max attenuation'),
+    'MIN_NN': (19, 13, 'float', 1, 0, 'rw', 'Gain-floor for non-stationary noise suppression.', '[-inf .. 0] dB (default: -10dB = 20log10(0.3))'),
+    'ECHOONOFF': (19, 14, 'int', 1, 0, 'rw', 'Echo suppression.', '0 = OFF', '1 = ON'),
+    'GAMMA_E': (19, 15, 'float', 3, 0, 'rw', 'Over-subtraction factor of echo (direct and early components). min .. max attenuation'),
+    'GAMMA_ETAIL': (19, 16, 'float', 3, 0, 'rw', 'Over-subtraction factor of echo (tail components). min .. max attenuation'),
+    'GAMMA_ENL': (19, 17, 'float', 5, 0, 'rw', 'Over-subtraction factor of non-linear echo. min .. max attenuation'),
+    'NLATTENONOFF': (19, 18, 'int', 1, 0, 'rw', 'Non-Linear echo attenuation.', '0 = OFF', '1 = ON'),
+    'NLAEC_MODE': (19, 20, 'int', 2, 0, 'rw', 'Non-Linear AEC training mode.', '0 = OFF', '1 = ON - phase 1', '2 = ON - phase 2'),
+    'SPEECHDETECTED': (19, 22, 'int', 1, 0, 'ro', 'Speech detection status.', '0 = false (no speech detected)', '1 = true (speech detected)'),
+    'FSBUPDATED': (19, 23, 'int', 1, 0, 'ro', 'FSB Update Decision.', '0 = false (FSB was not updated)', '1 = true (FSB was updated)'),
+    'FSBPATHCHANGE': (19, 24, 'int', 1, 0, 'ro', 'FSB Path Change Detection.', '0 = false (no path change detected)', '1 = true (path change detected)'),
+    'TRANSIENTONOFF': (19, 29, 'int', 1, 0, 'rw', 'Transient echo suppression.', '0 = OFF', '1 = ON'),
+    'VOICEACTIVITY': (19, 32, 'int', 1, 0, 'ro', 'VAD voice activity status.', '0 = false (no voice activity)', '1 = true (voice activity)'),
+    'STATNOISEONOFF_SR': (19, 33, 'int', 1, 0, 'rw', 'Stationary noise suppression for ASR.', '0 = OFF', '1 = ON'),
+    'NONSTATNOISEONOFF_SR': (19, 34, 'int', 1, 0, 'rw', 'Non-stationary noise suppression for ASR.', '0 = OFF', '1 = ON'),
+    'GAMMA_NS_SR': (19, 35, 'float', 3, 0, 'rw', 'Over-subtraction factor of stationary noise for ASR. ', '[0.0 .. 3.0] (default: 1.0)'),
+    'GAMMA_NN_SR': (19, 36, 'float', 3, 0, 'rw', 'Over-subtraction factor of non-stationary noise for ASR. ', '[0.0 .. 3.0] (default: 1.1)'),
+    'MIN_NS_SR': (19, 37, 'float', 1, 0, 'rw', 'Gain-floor for stationary noise suppression for ASR.', '[-inf .. 0] dB (default: -16dB = 20log10(0.15))'),
+    'MIN_NN_SR': (19, 38, 'float', 1, 0, 'rw', 'Gain-floor for non-stationary noise suppression for ASR.', '[-inf .. 0] dB (default: -10dB = 20log10(0.3))'),
+    'GAMMAVAD_SR': (19, 39, 'float', 1000, 0, 'rw', 'Set the threshold for voice activity detection.', '[-inf .. 60] dB (default: 3.5dB 20log10(1.5))'),
+    # 'KEYWORDDETECT': (20, 0, 'int', 1, 0, 'ro', 'Keyword detected. Current value so needs polling.'),
+    'DOAANGLE': (21, 0, 'int', 359, 0, 'ro', 'DOA angle. Current value. Orientation depends on build configuration.')
+}
+
+
+class Tuning:
+    def __init__(self):
+        self.TIMEOUT = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/TIMEOUT_MILLISECONDS", 100000)
+        self.dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
+
+        if not self.dev:
+            raise RuntimeError("Failed to find Respeaker device")
+        rospy.loginfo("Initializing Respeaker device")
+
+
+    def write(self, name, value):
+        try:
+            data = PARAMETERS[name]
+        except KeyError:
+            return
+
+        if data[5] == 'ro':
+            raise ValueError('{} is read-only'.format(name))
+
+        id = data[0]
+
+        # 4 bytes offset, 4 bytes value, 4 bytes type
+        if data[2] == 'int':
+            payload = struct.pack(b'iii', data[1], int(value), 1)
+        else:
+            payload = struct.pack(b'ifi', data[1], float(value), 0)
+
+        self.dev.ctrl_transfer()
+        # (
+        #     usb.util.CTRL_OUT | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+        #     0, 0, id, payload, self.TIMEOUT)
+
+    def read(self, name):
+        try:
+            data = PARAMETERS[name]
+        except KeyError:
+            return
+
+        id = data[0]
+
+        cmd = 0x80 | data[1]
+        if data[2] == 'int':
+            cmd |= 0x40
+
+        length = 8
+
+        response = self.dev.ctrl_transfer(
+            usb.util.CTRL_IN | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+            0, cmd, id, length, self.TIMEOUT)
+
+        response = struct.unpack(b'ii', response.tobytes())
+
+        if data[2] == 'int':
+            result = response[0]
+        else:
+            result = response[0] * (2.**response[1])
+
+        return result
+
+    def set_vad_threshold(self, db):
+        self.write('GAMMAVAD_SR', db)
+
+    def is_voice(self):
+        return self.read('VOICEACTIVITY')
+
+    @property
+    def direction(self):
+        return self.read('DOAANGLE')
+
+    @property
+    def version(self):
+        return self.dev.ctrl_transfer(
+            usb.util.CTRL_IN | usb.util.CTRL_TYPE_VENDOR | usb.util.CTRL_RECIPIENT_DEVICE,
+            0, 0x80, 0, 1, self.TIMEOUT)[0]
+
+    def close(self):
+        """
+        close the interface
+        """
+        usb.util.dispose_resources(self.dev)
+
+
+def get_respeaker_device_id():
+
+    with ignore_stderr():
+        p = pyaudio.PyAudio()
+    info = p.get_host_api_info_by_index(0)
+    num_devices = info.get('deviceCount')
+
+    device_id = -1
+    for i in range(num_devices):
+        if (p.get_device_info_by_host_api_device_index(0, i).get('maxInputChannels')) > 0:
+            if "ReSpeaker" in p.get_device_info_by_host_api_device_index(0, i).get('name'):
+                device_id = i
+
+    return device_id
+
+
+RESPEAKER_RATE = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/RESPEAKER_RATE", 16000)
+RESPEAKER_CHANNELS = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/RESPEAKER_CHANNELS", 6) # 6 must flash 6_channels_firmware.bin first
+RESPEAKER_WIDTH = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/RESPEAKER_WIDTH", 2)
+RESPEAKER_INDEX = get_respeaker_device_id()
+CHUNK = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/CHUNK", 1024)
+
+class ROSInterface:
+    def __init__(self):
+        # Initialiaze list to store audio segments
+        self.wav_list = []
+        self.record_count = 0 # Count how many times we've recorded f seconds of audio
+        self.secs = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/SECONDS")
+        self.chunk_size = rospy.get_param("/VZ_ACOUSTIC_SCENE_ANALYSIS/CHUNK_SIZE")
+        self.respeaker = Tuning()
+        # Publisher for Audio Data
+        self.audio_data_pub = rospy.Publisher("/wav_data", numpy_msg(VZ_AudioData), queue_size=10)
+        # For Utku's code: float of cough probabilty from sample
+        self.cough_prob = 90.000
+
+        self.model_path = '/home/hello-robot/vz_modules/NEU_VZ_ASA/MehrshadTesting/codes/A_CoughDetection/models/'
+        self.cough_classifier_file = self.model_path + 'cough_classifier'
+        self.cough_classifier_scaler = self.model_path + 'cough_classification_scaler'
+        self.loaded_model = pickle.load(open(self.cough_classifier_file, 'rb'))
+        self.loaded_scaler = pickle.load(open(self.cough_classifier_scaler, 'rb'))
+
+        # for testing audio collection
+        self.file_name = 'testing_audio_node6.wav'
+        self.dir = os.path.join('/home/hello-robot/clone/Robot_Autonomous_Navigation/catkin_ws/src/stretch_ros/vz_acoustic_scene_analysis/scripts/', self.file_name)
+
+
+
+    def get_audio(self):
+        return_list = False
+        [recorded_frames, len_of_frames] = self.record_audio(self.chunk_size) # set param here chunk size
+        self.wav_list.append(recorded_frames)
+        self.record_count = len(self.wav_list)
+        self.sequence_size = 11 # self.secs/self.chunk_size
+        if (self.record_count % self.sequence_size == 0):
+            return_list =  True
+            print("length")
+            print(len(self.wav_list))
+            print("Collected %d seconds of audio" %(self.secs))
+            # Remove fist two objects in array to remove 0.2 seconds of audio data from array
+    
+            # send the frames at the beginning of the list (save as wav for now)
+        return [return_list, len_of_frames]
+            
+
+
+    def record_audio(self, seconds=5):
+        p = pyaudio.PyAudio()
+
+        stream = p.open(rate=RESPEAKER_RATE,
+                        format=p.get_format_from_width(RESPEAKER_WIDTH),
+                        channels=RESPEAKER_CHANNELS,
+                        input=True,
+                        input_device_index=RESPEAKER_INDEX,
+                        output= False)
+
+        frames = []
+        arr_length = int(RESPEAKER_RATE / CHUNK * seconds) # get length of array to produce n seconds of audio
+        print("arr length")
+        print(arr_length)
+        for i in range(0, arr_length):
+            data = stream.read(CHUNK)
+            a = np.frombuffer(data,dtype=np.int16)[0::6] # extracts fused channel 0
+            self.audio_data_pub.publish(a)
+            # print(a)
+            frames.append(a)
+        
+        # Check to prevent empty frames list
+        if (len(frames) == 0):
+            # add garbage data
+            print("Having issues")
+            data[0] == 1
+        
+        stream.stop_stream()
+        stream.close()
+        p.terminate()
+
+        return [frames, arr_length]
+
+
+    def process_audio_loop(self):
+        dev = usb.core.find(idVendor=0x2886, idProduct=0x0018)
+        try:
+            if dev:
+                while not rospy.is_shutdown():
+                    if self.respeaker.is_voice() == 1:
+
+                        [iter, data_len] = self.get_audio()
+
+                        # if the wav list has values
+                        if(iter):
+                            # flatten list
+                            flat_list = [item for sublist in self.wav_list for item in sublist]
+                            flatter_list = [item for sublist in flat_list for item in sublist]
+
+                            flatter_list_np = np.asarray(flatter_list)
+
+                            # Test that the data is good
+                            scipy.io.wavfile.write(self.dir, 16000, flatter_list_np)
+
+                            # remove first 0.2 seconds from wav data
+                            self.wav_list.pop(0)
+                            self.wav_list.pop(0)
+
+                            # Call of Utku's function
+                            self.cough_prob = dsp.classify_cough(flatter_list_np,RESPEAKER_RATE,self.loaded_model,self.loaded_scaler)
+                            print("%d %% probability of cough" %round(self.cough_prob*100,2))
+                            # do something with probability (publish?)
+        except usb.core.USBError:
+            print('Respeaker not on USB bus')
+
+    def cleanup(self):
+        self.respeaker.close()
+        print("shutting down")
+
+
+if __name__ == "__main__":
+    try:
+        rospy.init_node("audio_capture")
+        audio = ROSInterface()
+        audio.process_audio_loop()
+    except rospy.ROSInterruptException:
+        print('Audio processing node failed!')
+        rospy.on_shutdown(audio.cleanup)

vz_acoustic_scene_analysis/setup.py

@@ -0,0 +1,8 @@
+from distutils.core import setup
+from catkin_pkg.python_setup import generate_distutils_setup
+
+d = generate_distutils_setup(
+    packages=['vz_acoustic_scene_analysis'],
+    package_dir={'': 'scripts'}
+)
+setup(**d)

vz_human_state_estimation/CMakeLists.txt

@@ -0,0 +1,209 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(vz_human_state_estimation)
+
+## 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
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  # opencv2 ananya : this does not compile
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+)
+
+## 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
+# )
+
+## 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
+#   sensor_msgs#   std_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 vz_human_state_estimation
+#  CATKIN_DEPENDS cv_bridge opencv2 roscpp rospy sensor_msgs std_msgs
+#  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}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/vz_human_state_estimation.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/vz_human_state_estimation_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
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## 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_vz_human_state_estimation.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)

vz_human_state_estimation/launch/human_state_estimation.launch

@@ -0,0 +1,7 @@
+<launch>
+    <node pkg="vz_human_state_estimation" type="ros_interface.py" name="human_state_estimation" />
+    <!-- flip images before passing onto human state estimation module -->
+    <param name="flip_images_human_state_est" type="bool" value="true" />
+    <param name="rgbd_topic" type="string" value="/D435i/image_raw" />
+    <param name="thermal_topic" type="string" value="/Lepton/image_raw" />
+</launch>

vz_human_state_estimation/package.xml

@@ -0,0 +1,77 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vz_human_state_estimation</name>
+  <version>0.0.0</version>
+  <description>The vz_human_state_estimation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ananya@todo.todo">ananya</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/vz_human_state_estimation</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">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>cv_bridge</build_depend>
+  <!-- <build_depend>opencv2</build_depend> -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <!-- <build_export_depend>opencv2</build_export_depend> -->
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <!-- <exec_depend>opencv2</exec_depend> -->
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

vz_human_state_estimation/scripts/merged_detection_algorithm.py

@@ -0,0 +1,216 @@
+#! /usr/bin/python3
+
+import cv2
+import imutils
+import numpy as np
+import math
+
+
+# Threshold value of the binary thresholding stage
+THRESH_VALUE = 120
+# The max threshold value each pixel below THRESH_VALUE is set to
+MAX_THRESH_VALUE = 255
+# Min and max values for contour areas of human body
+MIN_CNTR_HUMN_AREA = 4000
+MAX_CNTR_HUMN_AREA = 30000
+F_T=1.7681;
+F_RGB=1.93;
+
+xs_RGB=2.16;
+ys_RGB=3.84;
+xs_T=1.92;
+ys_T=1.44;
+
+Tx=150;
+Ty=150;
+
+
+
+HEAT_COLORMAP = 2
+
+
+class KalmanFilter(object):
+        def __init__(self, dt, u_x,u_y, std_acc, x_std_meas, y_std_meas):
+            """
+            :param dt: sampling time (time for 1 cycle)
+            :param u_x: acceleration in x-direction
+            :param u_y: acceleration in y-direction
+            :param std_acc: process noise magnitude
+            :param x_std_meas: standard deviation of the measurement in x-direction
+            :param y_std_meas: standard deviation of the measurement in y-direction
+            """
+            # Define sampling time
+            self.dt = dt
+            # Define the  control input variables
+            self.u = np.matrix([[u_x],[u_y]])
+            # Intial State
+            self.x = np.matrix([[0], [0], [0], [0]])
+            # Define the State Transition Matrix A
+            self.A = np.matrix([[1, 0, self.dt, 0],
+                                [0, 1, 0, self.dt],
+                                [0, 0, 1, 0],
+                                [0, 0, 0, 1]])
+            # Define the Control Input Matrix B
+            self.B = np.matrix([[(self.dt**2)/2, 0],
+                                [0, (self.dt**2)/2],
+                                [self.dt,0],
+                                [0,self.dt]])
+            # Define Measurement Mapping Matrix
+            self.H = np.matrix([[1, 0, 0, 0],
+                                [0, 1, 0, 0]])
+            #Initial Process Noise Covariance
+            self.Q = np.matrix([[(self.dt**4)/4, 0, (self.dt**3)/2, 0],
+                                [0, (self.dt**4)/4, 0, (self.dt**3)/2],
+                                [(self.dt**3)/2, 0, self.dt**2, 0],
+                                [0, (self.dt**3)/2, 0, self.dt**2]]) * std_acc**2
+            #Initial Measurement Noise Covariance
+            self.R = np.matrix([[x_std_meas**2,0],
+                               [0, y_std_meas**2]])
+            #Initial Covariance Matrix
+            self.P = np.eye(self.A.shape[1])
+
+            
+        def predict(self):
+                # Refer to :Eq.(9) and Eq.(10)  in https://machinelearningspace.com/object-tracking-simple-implementation-of-kalman-filter-in-python/?preview_id=1364&preview_nonce=52f6f1262e&preview=true&_thumbnail_id=1795
+                # Update time state
+                #x_k =Ax_(k-1) + Bu_(k-1)     Eq.(9)
+                self.x = np.dot(self.A, self.x) + np.dot(self.B, self.u)
+                # Calculate error covariance
+                # P= A*P*A' + Q               Eq.(10)
+                self.P = np.dot(np.dot(self.A, self.P), self.A.T) + self.Q
+                return self.x[0:2]
+
+        def update(self, z):
+               # Refer to :Eq.(11), Eq.(12) and Eq.(13)  in https://machinelearningspace.com/object-tracking-simple-implementation-of-kalman-filter-in-python/?preview_id=1364&preview_nonce=52f6f1262e&preview=true&_thumbnail_id=1795
+                    # S = H*P*H'+R
+                    S = np.dot(self.H, np.dot(self.P, self.H.T)) + self.R
+                    # Calculate the Kalman Gain
+                    # K = P * H'* inv(H*P*H'+R)
+                    K = np.dot(np.dot(self.P, self.H.T), np.linalg.inv(S))  #Eq.(11)
+                    self.x = np.round(self.x + np.dot(K, (z - np.dot(self.H, self.x))))   #Eq.(12)
+                    I = np.eye(self.H.shape[1])
+                    # Update error covariance matrix
+                    self.P = (I - (K * self.H)) * self.P   #Eq.(13)
+                    return self.x[0:2]
+
+
+
+
+def track (frame1, frame2, KF,hog,backSub):
+   
+    frame1 = imutils.resize(frame1, 
+                       width=min(300, frame1.shape[1]))
+    phi=math.pi
+    theta=math.pi
+    Ry=np.array([[math.cos(phi),0,math.sin(phi)],[0,1,0],[-math.sin(phi),0,math.cos(phi)]])
+    Rx=np.array([[1,0,0],[0,math.cos(theta),-math.sin(theta)],[0,math.sin(theta),math.cos(theta)]])
+    Rz=np.array([[math.cos(phi),0,math.sin(phi)],[0,1,0],[-math.sin(phi),0,math.cos(phi)]])
+
+    Cint_RGB= np.array([[F_RGB/xs_RGB , 0, frame1.shape[1]/2],[0,F_RGB/ys_RGB, frame1.shape[0]/2],[0,0,1]])
+    Cint_T= np.array([[F_T/xs_T , 0, frame2.shape[1]/2],[0,F_T/ys_T, frame2.shape[0]/2],[0,0,1]])
+    Cext_RGB_T=np.array([[1,0,Tx],[0,1,Ty],[0,0,1.5]])
+
+    H_RGB_T=np.matmul(Cint_RGB,np.matmul(Cext_RGB_T,np.linalg.inv(Cint_T)))
+    
+    frame2 = cv2.warpPerspective(frame2, H_RGB_T, [frame1.shape[1],frame1.shape[0]])
+    
+    #frame2 = imutils.resize(frame2, 
+                       #width=frame1.shape[1])
+    
+    # using a greyscale picture, also for faster detection
+    grayimg1 = cv2.cvtColor(frame1, cv2.COLOR_RGB2GRAY)
+  
+
+    #RGB detection
+    boxes, weights = hog.detectMultiScale(grayimg1, winStride=(8,8),scale=1.02)
+
+    boxes = np.array([[x, y, x + w, y + h] for (x, y, w, h) in boxes])
+     
+      
+    
+
+    #thermal detection
+    fgMask = backSub.apply(frame2)
+
+    kernel = np.ones((2,2),np.uint8)
+    fgMask = cv2.dilate(fgMask, kernel, iterations=2)
+
+    # Contour detection stage
+    contours, _ = cv2.findContours(fgMask, cv2.RETR_LIST, cv2.CHAIN_APPROX_NONE)
+
+    areas = [cv2.contourArea(c) for c in contours]
+    centers=[]
+    for idx, val in enumerate(areas):
+         for (xA1, yA1, xB1, yB1) in boxes:
+        # Human blob filtration stage
+            if MIN_CNTR_HUMN_AREA <= val <= MAX_CNTR_HUMN_AREA:
+                cntr = contours[idx]
+                # Fitting bounding boxes over our contours of interest (humans)
+                x,y,w,h = cv2.boundingRect(cntr)
+                # Final bounding box coordinates
+                xA2 = x
+                yA2 = y
+                xB2 = x+w
+                yB2 = y+h
+                if boxes.size==0:
+                    cv2.rectangle(frame1,(xA2,yA2),(xB2,yB2),(0,0,255),2)
+               
+                else :
+                    dx = min(xB1, xB2) - max(xA1, xA2)
+                    dy = min(yB1, yB2) - max(yA1, yA2)
+                    #cv2.rectangle(frame1,(xA1,yA1),(xB1,yB1),(0,255,0),2)
+                    cv2.rectangle(frame2,(xA2,yA2),(xB2,yB2),(0,0,255),2)
+                
+                    if (dx>=0) and (dy>=0):
+                        xA = max(xA1, xA2)
+                        yA = max(yA1, yA2)
+                        xB = min(xB1, xB2)
+                        yB = min(yB1, yB2)
+                        cv2.rectangle(frame1,(xA,yA),(xB,yB),(255,0,0),2)
+                        x=(xA+xB)/2
+                        y=(yA+yB)/2
+                        centers.append(np.array([[x],[y]]))
+   
+    if (len(centers) > 0):   
+            
+            (x1, y1) = KF.update(centers[0])
+    else:
+            (x1,y1)=KF.predict()
+
+    print(x1,y1)
+
+    cv2.rectangle(frame1, (int (x1) - 30, int (y1) - 80), (int (x1) + 30, int (y1) + 80), (0, 255, 0), 2)
+
+    return frame1
+
+
+# All sub classes for your code should be initialized in this class. 
+# e.g. Here we initialize the Kalman Filter, HOG, BackSub
+class ConnectToROS:
+    def __init__(self):
+
+        cv2.startWindowThread()
+
+        #create output video 
+        self.out = cv2.VideoWriter(
+            'output_rgb.avi',
+            cv2.VideoWriter_fourcc(*'MJPG'),
+            15.,
+            (640,480))
+
+        #initialize Kalman filter and HOG SVM
+        self.KF = KalmanFilter(0.01, 1, 0, 0, .1,.1)
+        self.hog = cv2.HOGDescriptor()
+        self.hog.setSVMDetector(cv2.HOGDescriptor_getDefaultPeopleDetector())
+        #background substractor
+        self.backSub = cv2.createBackgroundSubtractorMOG2()
+
+    # This is the main interface function between your code and ROS
+    # The overall algorithm takes in frame1: rgbd images, frame2: thermal images
+    # A third input parameter crucial not detailed here will be parameters not expected to change over time
+    # e.g. neural network weights. 
+    # So expect your function call to look like 'processData(input1,..,inputn, parameterlist)'
+    # Output : frame: Bounding Boxed image
+    def processData(self,frame1,frame2):
+        frame=track(frame1,frame2,self.KF,self.hog,self.backSub);
+        return frame

vz_human_state_estimation/scripts/ros_interface.py

@@ -0,0 +1,129 @@
+#! /usr/bin/python3
+
+# ROS specific imports 
+import rospy
+import message_filters
+from sensor_msgs.msg import Image
+from std_msgs.msg import String
+
+# Non ROS imports
+import merged_detection_algorithm as mda
+import cv2
+from cv_bridge import CvBridge, CvBridgeError
+
+UPDATE_RATE_HZ = 10 #hz for publishing bounding box later 
+
+class ROSInterface:
+    def __init__(self):
+        # Initialize the two images as None
+        self.cv_rgbd_img = None
+        self.cv_thermal_img = None
+        self.cv_bounding_boxed_img = None
+
+        # Images for display and passing to human state estimation node
+        # They will be rotated based on parameter /flip_images_human_state_est
+        self.cv_rgbd_img_probably_rotated = None
+        self.cv_thermal_img_probably_rotated = None
+
+        # To flip or not to flip...
+        self.flip_images_human_state_est = rospy.get_param('flip_images_human_state_est')
+
+        self.MDA = mda.ConnectToROS()
+
+        # set up CV Bridge
+        self.bridge = CvBridge()
+
+        # Get topic names
+        self.rgbd_topic = rospy.get_param("rgbd_topic")
+        self.thermal_topic = rospy.get_param("thermal_topic")
+
+        #Image subscribers
+        rgbd_sub    = message_filters.Subscriber(self.rgbd_topic, Image)
+        thermal_sub = message_filters.Subscriber(self.thermal_topic, Image)
+
+        # todo: confirm the slop parameter with Paul Ghanem. 2 secs seems high
+        ats = message_filters.ApproximateTimeSynchronizer([rgbd_sub,thermal_sub],queue_size=10,slop=2.0)
+        ats.registerCallback(self.ats_callback)
+    
+    # Once we receive rgbd and thermal images, which are also periodically received by this function
+    # We call this ats_callback function. This is also where we call the processData function to interface
+    # with other algorithms e.g. Human State Estimation and procure their outputs
+    def ats_callback(self,rgbd_img_data,thermal_img_data):
+        self.cv_rgbd_img = self.bridge.imgmsg_to_cv2(rgbd_img_data)
+        self.cv_thermal_img = self.bridge.imgmsg_to_cv2(thermal_img_data)
+
+        # Rotate if needed
+        # Show images and pass onto human state estimation flipped if following true
+        # This is done because the raw images captured from rgbd and thermal cameras are oriented incorrect
+
+        if(self.flip_images_human_state_est == True):
+            ##### NOTE: Change rotation based on what you see ########
+            self.cv_rgbd_img_probably_rotated = cv2.rotate(self.cv_rgbd_img,cv2.ROTATE_90_CLOCKWISE)            
+            self.cv_thermal_img_probably_rotated = cv2.rotate(self.cv_thermal_img,cv2.ROTATE_180)
+        else:
+            self.cv_rgbd_img_probably_rotated = self.cv_rgbd_img
+            self.cv_thermal_img_probably_rotated = self.cv_thermal_img
+
+        # Call the human state estimation code via processData interface
+        # This is where we will iteratively call the algorithms to obtain outputs eg. bounding boxes
+        self.cv_bounding_boxed_img = self.MDA.processData(self.cv_rgbd_img_probably_rotated,self.cv_thermal_img_probably_rotated)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            if(self.cv_rgbd_img_probably_rotated is None or self.cv_thermal_img_probably_rotated is None or self.cv_bounding_boxed_img is None):
+                continue
+            try:
+                # Show images
+                cv2.imshow('D435i',cv2.resize(self.cv_rgbd_img_probably_rotated,(600,800), interpolation = cv2.INTER_AREA))
+                cv2.imshow('Thermal',cv2.resize(self.cv_thermal_img_probably_rotated,(600,800), interpolation = cv2.INTER_AREA))
+                cv2.imshow('BoundingBox',cv2.resize(self.cv_bounding_boxed_img,(600,800), interpolation = cv2.INTER_AREA))
+                cv2.waitKey(3)
+
+            except Exception as e:
+                rospy.logwarn(e)
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('merged_detection_algorithm_ros', anonymous=True)
+        interface = ROSInterface()
+        interface.run()
+    except rospy.ROSInterruptException:
+        print('Human Pose Detection Node Failed')
+        pass
+    cv2.destroyAllWindows()
+
+
+# The following is just a simple subscriber. I think we need a time synced subscriber
+# class ROSInterface:
+#     def __init__(self):
+#         # Initialize the two images as None
+#         self.cv_rgbd_img = None
+#         self.cv_thermal_img = None
+
+#         # set up CV Bridge
+#         self.bridge = CvBridge()
+
+#         #Image subscribers
+#         rospy.Subscriber("/D435i_raw",Image,self.rgbd_callback)
+#         rospy.Subscriber("/flir_3_5_near_realsense_raw",Image,self.thermal_callback)
+    
+#     def rgbd_callback(self,rgbd_img_data):
+#         self.cv_rgbd_img = self.bridge.imgmsg_to_cv2(rgbd_img_data)
+#         print('rgbd callback')
+        
+#     def thermal_callback(self,thermal_img_data):
+#         self.cv_thermal_img = self.bridge.imgmsg_to_cv2(thermal_img_data)
+#         print('thermal callback')
+    
+#     def run(self):
+#         while not rospy.is_shutdown():
+#             if(self.cv_rgbd_img is None or self.cv_thermal_img is None):
+#                 continue
+#             try:
+#                 # Show images
+#                 cv2.imshow('D435i',cv2.resize(self.cv_rgbd_img,(600,800), interpolation = cv2.INTER_AREA))
+#                 cv2.imshow('Thermal',cv2.resize(self.cv_thermal_img,(600,800), interpolation = cv2.INTER_AREA))
+#                 cv2.waitKey(3)
+
+#             except Exception as e:
+#                 rospy.logwarn(e)

vz_optical_remote_vital_sensing/scripts/orvs_ros_interface.py

@@ -0,0 +1,55 @@
+#! /usr/bin/python3
+
+# ROS imports
+import rospy
+import numpy as np
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float64
+
+# Python imports
+from cv_bridge import CvBridge, CvBridgeError
+import time
+import cv2
+
+# Custom imports
+import VZ_ORVS.Thermal_Video_Submodule.Thermal_Camera_PostProcessing_Demo as pp_demo
+
+class FLIR_LEPTON:
+    def __init__(self):
+        # Intialize empty stores for heart rate or breathes per min
+        self.bpm = None
+
+        self.bridge = CvBridge()
+        self.video_writer = None
+        self.file_name = 'bpm_vid.avi'
+        self.fps = 20 # need to find actual value
+
+        self.thermal_topic = rospy.get_param("thermal_topic")
+        self.bpm_topic = rospy.get_param("bpm_topic")
+        
+        self.thermal_sub = rospy.Subscriber(self.thermal_topic, Image, self.thermal_cb)
+        self.bpm_pub = rospy.Publisher(self.bpm_topic, Float64, queue_size=10)
+
+    def thermal_cb(self, data):
+        # get image data into mp4 form
+        cv_image = self.bridge.imgmsg_to_cv2(data, 'bgr8')
+
+        if self.video_writer is None:
+            rows, cols, _ = cv_image.shape
+            self.video_writer = cv2.VideoWriter(self.file_name, -1, self.fps, (cols, rows))
+        
+        self.video_writer.write(cv_image)
+
+    def cleanup(self):
+        if self.video_writer is not None:
+            self.video_writer.release()
+
+if __name__== '__main__':
+    try:
+        rospy.init_node("bpm_capture")
+        processor = FLIR_LEPTON()
+        processor.run()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        print('BPM extraction node failed!')
+        rospy.on_shutdown(processor.cleanup)

vz_ros_wrappers/CMakeLists.txt

@@ -0,0 +1,210 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(vz_ros_wrappers)
+
+## 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
+find_package(catkin REQUIRED COMPONENTS
+  cv_bridge
+  # opencv2 # ananya this does not compile
+  roscpp
+  rospy
+  sensor_msgs
+  std_msgs
+)
+
+find_package(PkgConfig)
+pkg_check_modules(GST1.0 gstreamer-1.0 REQUIRED)
+
+## System dependencies are found with CMake's conventions
+find_package(Boost REQUIRED COMPONENTS thread)
+
+
+## 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
+# )
+
+## 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
+#   sensor_msgs#   std_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 vz_ros_wrappers
+#  CATKIN_DEPENDS cv_bridge opencv2 roscpp rospy sensor_msgs std_msgs
+#  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}
+  ${Boost_INCLUDE_DIRS}
+  ${GST1.0_INCLUDE_DIRS}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/vz_ros_wrappers.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
+
+
+
+## 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
+
+## Specify libraries to link a library or executable target against
+
+
+#############
+## 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
+#   scripts/my_python_script
+#   DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+# )
+
+## Mark executables for installation
+## See http://docs.ros.org/melodic/api/catkin/html/howto/format1/building_executables.html
+
+
+## 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 launch
+   DESTINATION ${CATKIN_PACKAGE_SHARE_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_vz_ros_wrappers.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)

vz_ros_wrappers/README.md

@@ -0,0 +1,5 @@
+# Following ROS Wrappers inside this package
+- FLIR_Lepton ROS Wrapper
+ROS Wrapper for FLIR Lepton Camera interfacing through [GroupGets DevBoard](https://groupgets.com/manufacturers/getlab/products/purethermal-2-flir-lepton-smart-i-o-module). This is inside grab_thermal.py file
+- ROS Wrapper for D435i intel realsense camera. Accesses the connection via USB and converts opencv images to ROS . This is inside grab_D435i.py file
+- ROS Wrapper for respeasker. (Todo: Ask Utku Demir what his code looks like, I/O etc)

vz_ros_wrappers/launch/D435i_and_Lepton.launch

@@ -0,0 +1,13 @@
+<launch>
+      <node pkg="vz_ros_wrappers" type="grab_D435i.py" name="D435i" />
+      <node pkg="vz_ros_wrappers" type="grab_thermal.py" name="FLIRLepton" />
+      <node pkg="vz_ros_wrappers" type="orvs_ros_interface.py" name="bpm_capture" output="screen" />
+      <node pkg="image_transport" type="republish" name="D435iCompressed"
+            args="raw in:=/D435i/image_raw compressed out:=/D435i/image_raw" />
+      <!-- Flip images and display when collecting data? -->
+      <param name="flip_images_grab_data" type="bool" value="true" />
+      <param name="rgbd_topic" type="string" value="/D435i/image_raw" />
+      <param name="thermal_topic" type="string" value="/Lepton/image_raw" />    
+      <param name="bpm_topic" type="string" value="/Lepton/bpm" /> 
+      <param name="rec_secs" type="int" value="10" /> 
+  </launch>

vz_ros_wrappers/package.xml

@@ -0,0 +1,76 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vz_ros_wrappers</name>
+  <version>0.0.0</version>
+  <description>The vz_ros_wrappers package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="ananya@todo.todo">ananya</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/vz_ros_wrappers</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">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>cv_bridge</build_depend>
+  <!-- <build_depend>opencv2</build_depend> -->
+  <build_depend>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>sensor_msgs</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>cv_bridge</build_export_depend>
+  <!-- <build_export_depend>opencv2</build_export_depend> -->
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>sensor_msgs</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>cv_bridge</exec_depend>
+  <!-- <exec_depend>opencv2</exec_depend> -->
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

vz_ros_wrappers/scripts/grab_D435i.py

@@ -0,0 +1,111 @@
+#! /usr/bin/python3
+'''
+Derived this from Nathaniel Hanson : https://github.com/RIVeR-Lab/flir_lepton
+Purpose: Capture images from D435i and republish. This is only for sim. Stretch has its own driver
+'''
+
+# Future proofing python 2
+from __future__ import nested_scopes
+from __future__ import generators
+from __future__ import division
+from __future__ import absolute_import
+from __future__ import with_statement
+from __future__ import print_function
+# Standard package imports
+import rospy
+import tf2_ros
+import sys
+import os
+import cv2
+import roslib
+import math
+import traceback
+import numpy as np
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import time
+
+UPDATE_RATE = 33333333.333 # nanoseconds
+
+class D435i:
+    def __init__(self):
+        # Intialize empty stores for current image
+        self.D435i_image = None
+        # The following image will be flipped or not based on parameter /flip_images_grab_data
+        self.D4355i_image_for_display = None
+
+        # To flip or not to flip..
+        self.flip_images_grab_data = rospy.get_param('flip_images_grab_data')
+
+        # setup subscribed topics
+        # initialize ros/cv2 bridge
+        
+        # Note: when testing with your own video, replace with local file
+        self.D435i_cap = cv2.VideoCapture('/dev/video4')
+        # self.D435i_cap = cv2.VideoCapture('/home/ananya/Downloads/2022-02-09-16-20-58(2).mp4')
+
+        # Get topic parameter
+        self.rgbd_topic = rospy.get_param("rgbd_topic")
+
+        if not self.D435i_cap.isOpened():
+            raise(Exception,'Unable to open video stream')
+        self.bridge = CvBridge()
+        self.D435i_pub = rospy.Publisher(self.rgbd_topic, Image, queue_size=100)
+        self.timer = rospy.Timer(rospy.Duration(0, UPDATE_RATE), self.publish_image)
+        self.timer_display = rospy.Timer(rospy.Duration(0, UPDATE_RATE), self.display_images)
+
+    def publish_image(self, timer):
+        try:
+            if self.D435i_image is None:
+                return
+
+            img_msg = self.bridge.cv2_to_imgmsg(self.D435i_image,encoding="passthrough")
+            img_msg.header.frame_id = 'D435i'
+            img_msg.header.stamp = rospy.Time.now()
+
+            self.D435i_pub.publish(img_msg)
+
+        except Exception as e:
+            print(traceback.print_exc())
+
+    # todo ananya: comment out function below later once fully ros integrated
+    def display_images(self, timer):
+        if self.D435i_image is None:
+            return
+        try:
+            # Show images
+            if(self.flip_images_grab_data == True):
+                ##### NOTE: Change rotation based on what you see ########
+                self.D4355i_image_for_display = cv2.rotate(self.D435i_image,cv2.ROTATE_90_CLOCKWISE)
+                ################################################################
+            else:
+                self.D4355i_image_for_display = self.D435i_image
+            
+            cv2.imshow('D435i',cv2.resize(self.D4355i_image_for_display,(600,800), interpolation = cv2.INTER_AREA))
+            cv2.waitKey(3)
+
+        except Exception as e:
+            rospy.logwarn(e)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            ret, self.D435i_image = self.D435i_cap.read()
+            
+            # ##### NOTE: Change rotation based on what you see ########
+            # self.D435i_image = cv2.rotate(self.D435i_image,cv2.ROTATE_90_CLOCKWISE)
+            # ################################################################
+            
+            # Note : To read the input at the same frame rate as the recorded video, when using 
+            # locally recorded video, uncomment the following and replace with (1/frame rate of video)
+            # video_frame_rate_to_freq = 1/9
+            # time.sleep(video_frame_rate_to_freq)
+
+if __name__ == '__main__':
+    try:
+        rospy.init_node('D435i', anonymous=True)
+        processor = D435i()
+        processor.run()
+    except rospy.ROSInterruptException:
+        print('Image processing node failed!')
+        pass
+    cv2.destroyAllWindows()

vz_ros_wrappers/scripts/grab_thermal.py

@@ -0,0 +1,117 @@
+#! /usr/bin/python3
+'''
+Initial Author: Nathaniel Hanson
+Date: 11/29/2021
+File: grab_thermal.py
+
+Purpose: Capture images from FLIR Lepton dev kit and republish
+'''
+
+# Future proofing python 2
+from __future__ import nested_scopes
+from __future__ import generators
+from __future__ import division
+from __future__ import absolute_import
+from __future__ import with_statement
+from __future__ import print_function
+# Standard package imports
+import rospy
+import tf2_ros
+import sys
+import os
+import cv2
+import roslib
+import math
+import traceback
+import numpy as np
+from sensor_msgs.msg import Image
+from cv_bridge import CvBridge, CvBridgeError
+import time
+
+UPDATE_RATE = 33333333.333 # nanoseconds
+
+class FLIR_LEPTON:
+    def __init__(self):
+        # Intialize empty stores for current image
+        self.thermal_image = None
+        # The following image will be flipped or not based on parameter /flip_images_grab_data
+        self.thermal_image_for_display = None
+
+        # To flip or not to flip..
+        self.flip_images_grab_data = rospy.get_param('flip_images_grab_data')
+
+        # setup subscribed topics
+        # initialize ros/cv2 bridge
+        
+        # the following depends on usb port we plug into
+        # for the vz project stretch robot, the top aux usb is video6
+        # Note: when testing with your own video, replace with local file
+
+        self.thermal_cap = cv2.VideoCapture('/dev/video6')
+        # self.thermal_cap = cv2.VideoCapture('/home/ananya/Downloads/Thermal-3(2).mp4')
+
+        # Get topic parameter
+        self.thermal_topic = rospy.get_param("thermal_topic")
+        
+        if not self.thermal_cap.isOpened():
+            raise(Exception,'Unable to open video stream')
+        self.bridge = CvBridge()
+        self.thermal_pub = rospy.Publisher(self.thermal_topic, Image, queue_size=100)
+        self.timer = rospy.Timer(rospy.Duration(0, UPDATE_RATE), self.publish_image)
+        self.timer_display = rospy.Timer(rospy.Duration(0, UPDATE_RATE), self.display_images)
+
+    def publish_image(self, timer):
+        try:
+            if self.thermal_image is None:
+                return
+                
+            img_msg = self.bridge.cv2_to_imgmsg(self.thermal_image, encoding="passthrough")
+            img_msg.header.frame_id = 'flir_3_5_near_realsense'
+            img_msg.header.stamp = rospy.Time.now()
+            self.thermal_pub.publish(img_msg)
+            
+        except Exception as e:
+            print(traceback.print_exc())
+
+    # todo ananya: comment out function below later once fully ros integrated
+    def display_images(self, timer):
+        if self.thermal_image is None:
+            return
+        try:
+            # Show images
+            if(self.flip_images_grab_data == True):
+                ##### NOTE: Change rotation based on what you see ########
+                self.thermal_image_for_display = cv2.rotate(self.thermal_image,cv2.ROTATE_180)
+                ################################################################
+            else:
+                self.thermal_image_for_display = self.thermal_image
+
+            cv2.imshow('Lepton',cv2.resize(self.thermal_image_for_display,(600,800), interpolation = cv2.INTER_AREA))
+            cv2.waitKey(3)
+
+        except Exception as e:
+            rospy.logwarn(e)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            ret, self.thermal_image = self.thermal_cap.read()
+
+            # ##### NOTE: Change rotation based on what you see ########
+            # self.thermal_image = cv2.rotate(self.thermal_image,cv2.ROTATE_180)
+            # ################################################################
+
+            # Note : To read the input at the same frame rate as the recorded video, when using 
+            # locally recorded video, uncomment the following and replace with (1/frame rate of video)
+            # Comment the following two lines when running on the robot
+            # video_frame_rate_to_freq = 1/7
+            # time.sleep(video_frame_rate_to_freq)
+           
+if __name__ == '__main__':
+    try:
+        rospy.init_node('FLIRLepton', anonymous=True)
+        processor = FLIR_LEPTON()
+        processor.run()
+    except rospy.ROSInterruptException:
+        print('Image processing node failed!')
+        pass
+    cv2.destroyAllWindows()

vz_ros_wrappers/scripts/orvs_ros_interface.py

@@ -0,0 +1,69 @@
+#! /usr/bin/python3
+
+# ROS imports
+import rospy
+import numpy as np
+from sensor_msgs.msg import Image
+from std_msgs.msg import Float64
+
+# Python imports
+from cv_bridge import CvBridge, CvBridgeError
+
+# Custom imports
+from VZ_ORVS.Thermal_Video_Submodule.Thermal_Camera_PostProcessing_Demo import AVI as avi
+
+class FLIR_LEPTON:
+    def __init__(self):
+        # Intialize empty stores for heart rate or breathes per min
+        self.bpm = None
+
+        self.bridge = CvBridge()
+        self.video_writer = None
+
+        self.fps = 30
+        self.seconds = rospy.get_param("rec_secs")
+        self.num_frames = self.fps * self.seconds
+        self.frame_arr = [None]*(self.num_frames)
+        self.time_arr = [None]*(self.num_frames)
+        
+        self.index = 0
+        self.t_zero = 0
+
+        self.thermal_topic = rospy.get_param("thermal_topic")
+        self.bpm_topic = rospy.get_param("bpm_topic")
+        
+        self.thermal_sub = rospy.Subscriber(self.thermal_topic, Image, self.thermal_cb)
+        self.bpm_pub = rospy.Publisher(self.bpm_topic, Float64, queue_size=10)
+
+
+    def thermal_cb(self, msg_data):
+        cv_image = self.bridge.imgmsg_to_cv2(msg_data, desired_encoding='passthrough')
+        t = msg_data.header.stamp
+        if self.index == 0:
+            self.t_zero = t.to_sec()
+        if self.index < self.num_frames:
+            print(self.index)
+            self.frame_arr[self.index] = cv_image
+            deltat = t.to_sec() - self.t_zero
+            self.time_arr[self.index] = deltat
+            self.index += 1
+        else: # when list is full
+            self.index = 0 # reset index
+            vid_arr = np.asarray(self.frame_arr) # make into np array 
+            print("sending to Rahul's code")
+            avi.get_bpm(self,vid_arr,self.time_arr) # perform bpm measurement
+            
+
+    def cleanup(self):
+        if self.video_writer is not None:
+            self.video_writer.release()
+
+
+if __name__== '__main__':
+    try:
+        rospy.init_node("bpm_capture")
+        processor = FLIR_LEPTON()
+        rospy.spin()
+    except rospy.ROSInterruptException:
+        print('BPM extraction node failed!')
+        rospy.on_shutdown(processor.cleanup)

vz_stretch_navigation/CMakeLists.txt

@@ -0,0 +1,207 @@
+cmake_minimum_required(VERSION 3.0.2)
+project(vz_stretch_navigation)
+
+## 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
+find_package(catkin REQUIRED COMPONENTS
+  roscpp
+  rospy
+  std_msgs
+  move_base
+)
+
+## 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
+# )
+
+## 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
+# )
+
+################################################
+## 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 vz_stretch_navigation
+#  CATKIN_DEPENDS roscpp rospy std_msgs
+#  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}
+)
+
+## Declare a C++ library
+# add_library(${PROJECT_NAME}
+#   src/${PROJECT_NAME}/vz_stretch.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/vz_stretch_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
+  scripts/simple_navigation_goal_client.py
+  DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION}
+)
+
+## 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_vz_stretch.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)

vz_stretch_navigation/config/base_local_planner_params.yaml

@@ -0,0 +1,14 @@
+DWAPlannerROS:
+    max_vel_x: 0.4
+    min_vel_x: -0.2
+    max_vel_theta: 0.4
+
+    acc_lim_th: 1.5
+    acc_lim_x: 1.5
+    
+    yaw_goal_tolerance: 0.1
+    xy_goal_tolerance: 0.1
+
+    holonomic_robot: false
+
+    path_distance_bias: 50.0

vz_stretch_navigation/config/common_costmap_params.yaml

@@ -0,0 +1,8 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+robot_radius: 0.1
+inflation_radius: 0.25
+
+observation_sources: laser_scan_sensor
+
+laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan_filtered, marking: true, clearing: true}

vz_stretch_navigation/config/common_costmap_params_gazebo.yaml

@@ -0,0 +1,9 @@
+obstacle_range: 2.5
+raytrace_range: 3.0
+#robot is square ~13.5*13.5 inchfootprint. This 0.2 is a circle with the same area
+robot_radius: 0.20 #robot is square ~13.5*13.5 inchfootprint. This 0.2 is a circle with the same area
+inflation_radius: 0.25
+
+observation_sources: laser_scan_sensor
+# the change in this and hardware file is the scan topic used here as opposed to scan_filtered
+laser_scan_sensor: {sensor_frame: laser, data_type: LaserScan, topic: scan, marking: true, clearing: true}

vz_stretch_navigation/config/global_costmap_params_nomap.yaml

@@ -0,0 +1,5 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  static_map: false

vz_stretch_navigation/config/global_costmap_params_withmap.yaml

@@ -0,0 +1,5 @@
+global_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  static_map: true

vz_stretch_navigation/config/local_costmap_params.yaml

@@ -0,0 +1,10 @@
+local_costmap:
+  global_frame: map
+  robot_base_frame: base_link
+  update_frequency: 5.0
+  publish_frequency: 2.0
+  static_map: false
+  rolling_window: true
+  width: 6.0
+  height: 6.0
+  resolution: 0.05

vz_stretch_navigation/config/teb_local_planner_params.yaml

@@ -0,0 +1,14 @@
+TebLocalPlannerROS:
+    max_vel_x: 0.4
+    max_vel_theta: 0.4
+
+    acc_lim_theta: 1.5
+    acc_lim_x: 1.5
+    acc_lim_y: 1.5
+    
+    yaw_goal_tolerance: 0.05
+    xy_goal_tolerance: 0.1
+
+    holonomic_robot: false
+
+    global_plan_viapoint_sep: 0.1

vz_stretch_navigation/config/teb_local_planner_params_gazebo.yaml

@@ -0,0 +1,85 @@
+# Copy pasted from 
+# http://wiki.ros.org/teb_local_planner/Tutorials/Configure%20and%20run%20Robot%20Navigation
+
+TebLocalPlannerROS:
+# todo: check if these two make sense
+ odom_topic: odom
+ map_frame: /odom
+    
+ # Trajectory
+  
+ teb_autosize: True
+ dt_ref: 0.3
+ dt_hysteresis: 0.1
+ global_plan_overwrite_orientation: True
+ max_global_plan_lookahead_dist: 3.0
+ feasibility_check_no_poses: 5
+    
+ # Robot
+         
+ max_vel_x: 1.5 #0.4 todo: ananya change this back
+ max_vel_x_backwards: 0.2
+ max_vel_theta: 0.3
+ acc_lim_x: 0.2 #0.5
+ acc_lim_theta: 0.25 #0.5 changed this ananya
+ min_turning_radius: 0.0
+ footprint_model: # types: "point", "circular", "two_circles", "line", "polygon"
+   type: "point"
+   radius: 0.2 # for type "circular"
+   line_start: [-0.3, 0.0] # for type "line"
+   line_end: [0.3, 0.0] # for type "line"
+   front_offset: 0.2 # for type "two_circles"
+   front_radius: 0.2 # for type "two_circles"
+   rear_offset: 0.2 # for type "two_circles"
+   rear_radius: 0.2 # for type "two_circles"
+   vertices: [ [0.25, 0.05] ] # for type "polygon"
+  #  vertices: [ [0.25, -0.05], [0.18, -0.05], [0.18, -0.18], [-0.19, -0.18], [-0.25, 0], [-0.19, 0.18], [0.18, 0.18], [0.18, 0.05], [0.25, 0.05] ] # for type "polygon"
+
+ # GoalTolerance
+    
+ xy_goal_tolerance: 0.2
+ yaw_goal_tolerance: 0.1
+ free_goal_vel: False
+    
+ # Obstacles
+    
+ min_obstacle_dist: 0.4 #this needs to change todo ananya
+ include_costmap_obstacles: True
+ costmap_obstacles_behind_robot_dist: 1.0
+ obstacle_poses_affected: 30
+ costmap_converter_plugin: ""
+ costmap_converter_spin_thread: True
+ costmap_converter_rate: 5
+
+ # Optimization
+    
+ no_inner_iterations: 5
+ no_outer_iterations: 4
+ optimization_activate: True
+ optimization_verbose: False
+ penalty_epsilon: 0.1 #todo : ananya this has something to do with allowing cost violations. look into this
+ weight_max_vel_x: 2
+ weight_max_vel_theta: 1
+ weight_acc_lim_x: 1
+ weight_acc_lim_theta: 10 #1 changed this ananya
+ weight_kinematics_nh: 1000
+ weight_kinematics_forward_drive: 1
+ weight_kinematics_turning_radius: 1
+ weight_optimaltime: 1
+ weight_obstacle: 50 #todo : ananya this has something to do with allowing cost violations. look into this
+ weight_dynamic_obstacle: 10 # not in use yet
+ alternative_time_cost: False # not in use yet
+
+ # Homotopy Class Planner
+
+ enable_homotopy_class_planning: True
+ enable_multithreading: True
+ simple_exploration: False
+ max_number_classes: 4
+ roadmap_graph_no_samples: 15
+ roadmap_graph_area_width: 5
+ h_signature_prescaler: 0.5
+ h_signature_threshold: 0.1
+ obstacle_keypoint_offset: 0.1
+ obstacle_heading_threshold: 0.45
+ visualize_hc_graph: False

vz_stretch_navigation/launch/mapping.launch

@@ -0,0 +1,24 @@
+<launch>
+
+  <arg name="teleop_type" default="keyboard" doc="how to teleop ('keyboard', 'joystick', or 'none')" />
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true"/>
+  <param name="/stretch_driver/mode" type="string" value="navigation" />
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true"/>
+
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
+
+  <!-- TELEOP -->
+  <include file="$(find stretch_core)/launch/teleop_twist.launch">
+    <arg name="teleop_type" value="$(arg teleop_type)" />
+    <arg name="linear" value="0.04" />
+    <arg name="angular" value="0.1" />
+    <arg name="twist_topic" value="/stretch/cmd_vel" />
+  </include>
+
+  <!-- MAPPING -->
+  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="log" />
+
+</launch>

vz_stretch_navigation/launch/mapping_gazebo.launch

@@ -0,0 +1,30 @@
+<launch>
+
+  <arg name="rviz"                   default="true"                      doc="whether to show Rviz" />
+  <arg name="teleop_type"            default="keyboard"                  doc="how to teleop ('keyboard', 'joystick', or 'none')" />
+  <arg name="gazebo_world"           default="worlds/willowgarage.world" doc="the environment within which Stretch is loaded in Gazebo" />
+  <arg name="gazebo_gpu_lidar"       default="false"                     doc="whether to compute lidar with hardware acceleration (requires GPU)" />
+  <arg name="gazebo_visualize_lidar" default="false"                     doc="whether to visualize planar lidar within Gazebo" />
+
+  <!-- GAZEBO SIMULATION -->
+  <include file="$(find stretch_gazebo)/launch/gazebo.launch">
+    <arg name="world" value="$(arg gazebo_world)" />
+    <arg name="visualize_lidar" value="$(arg gazebo_visualize_lidar)" />
+    <arg name="gpu_lidar" value="$(arg gazebo_gpu_lidar)" />
+  </include>
+
+  <!-- TELEOP -->
+  <include file="$(find stretch_core)/launch/teleop_twist.launch">
+    <arg name="teleop_type" value="$(arg teleop_type)" />
+    <arg name="linear" value="1.0" />
+    <arg name="angular" value="2.0" />
+    <arg name="twist_topic" value="/stretch_diff_drive_controller/cmd_vel" />
+  </include>
+
+  <!-- MAPPING -->
+  <node pkg="gmapping" type="slam_gmapping" name="gmapping_record_map" output="log" />
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />
+
+</launch>

vz_stretch_navigation/launch/mapping_rviz.launch

@@ -0,0 +1,4 @@
+<launch>
+    <arg name="rviz" default="true" doc="whether to show Rviz" />
+    <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find vz_stretch_navigation)/rviz/mapping.rviz" if="$(arg rviz)" />
+</launch>

vz_stretch_navigation/launch/navigation.launch

@@ -0,0 +1,30 @@
+<launch>
+
+  <arg name="map_yaml" doc="filepath to previously captured map (required)" />
+
+  <!-- STRETCH DRIVER -->
+  <param name="/stretch_driver/broadcast_odom_tf" type="bool" value="true" />
+  <param name="/stretch_driver/mode" type="string" value="navigation" />
+  <include file="$(find stretch_core)/launch/stretch_driver.launch" pass_all_args="true" />
+
+  <!-- LASER RANGE FINDER -->
+  <include file="$(find stretch_core)/launch/rplidar.launch" />
+
+  <!-- MAP SERVER -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
+
+  <!-- LOCALIZATION -->
+  <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+  <!-- NAVIGATION -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <rosparam file="$(find vz_stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find vz_stretch_navigation)/config/common_costmap_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find vz_stretch_navigation)/config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find vz_stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
+    <rosparam file="$(find vz_stretch_navigation)/config/base_local_planner_params.yaml" command="load" />
+    <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" />
+    <remap from="/cmd_vel" to="/stretch/cmd_vel" />
+  </node>
+
+</launch>

vz_stretch_navigation/launch/navigation/map_server.launch

@@ -0,0 +1,8 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/map_server?distro=kinetic -->
+
+  <arg name="map_name" default="map" />
+
+  <!-- launch map server -->
+  <node name="map_server" pkg="map_server" type="map_server" output="screen" args="$(arg map_name)"/>
+</launch>

vz_stretch_navigation/launch/navigation/move_base.launch

@@ -0,0 +1,30 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/move_base?distro=kinetic -->
+
+  <arg name="config"/>
+
+  <arg name="cmd_vel_topic"/>
+  <arg name="odom_topic"/>
+
+  <!-- Use global costmap-->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <!-- Include robot specific parameters from the robot config folder -->
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/costmap_common_params.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/costmap_height.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/global_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/costmap_common_params.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/costmap_height.yaml" command="load" ns="local_costmap" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/base_local_planner_params.yaml" command="load" />
+    <rosparam file="$(find stretch_navigation)/resources/config/$(arg config)/global_planner_params.yaml" command="load" />
+
+    <param name="conservative_reset_dist" type="double" value="3.0" />
+    <param name="controller_frequency" type="double" value="7.0" /> <!-- 20 -->
+    <param name="planner_patience" value="10.0" />
+    <param name="max_planning_retries" value="20" />
+
+    <remap from="cmd_vel" to="$(arg cmd_vel_topic)" />
+    <remap from="odom" to="$(arg odom_topic)" />
+  </node>
+
+</launch>

vz_stretch_navigation/launch/navigation/octomap_server.launch

@@ -0,0 +1,33 @@
+<launch>
+  <!-- Doc: http://wiki.ros.org/octomap_server?distro=kinetic -->
+
+  <arg name="map" default="map" />
+  <arg name="base_link" default="base_link" />
+
+  <!-- launch server for positive map-->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server" output="screen" args="$(arg map)" >
+    <remap from="cloud_in" to="pointcloud_static_octomap_ignore" />
+    <param name="base_frame_id" value="$(arg base_link)" />
+    <param name="frame_id" type="string" value="map" />
+    <param name="occupancy_min_z" value="0.23" />
+    <param name="occupancy_max_z" value="2" />
+  </node>
+
+  <!-- launch server for negative map-->
+  <node pkg="octomap_server" type="octomap_server_node" name="octomap_server_negative" output="screen" args="$(arg map)" >
+    <remap from="cloud_in" to="pointcloud_static_octomap_ignore" />
+    <param name="base_frame_id" value="$(arg base_link)" />
+    <param name="frame_id" type="string" value="map" />
+    <param name="occupancy_max_z" value="-0.23" />
+    <param name="occupancy_min_z" value="-10" />
+
+    <!-- rename all colliding topics and services-->
+    <remap from="octomap_binary" to="octomap_binary_negative" />
+    <remap from="octomap_full" to="octomap_full_negative" />
+    <remap from="occupied_cells_vis_array" to="occupied_cells_vis_array_negative" />
+    <remap from="octomap_point_cloud_centers" to="octomap_point_cloud_centers_negative" />
+    <remap from="projected_map" to="projected_map_negative" />
+    <remap from="clear_bbx" to="clear_bbx_negative" />
+    <remap from="reset" to="reset_negative" />
+  </node>
+</launch>

vz_stretch_navigation/launch/navigation_3d.launch

@@ -0,0 +1,46 @@
+<launch>
+  <!-- The navigation_3d launch file includes all of the launch files needed for a robot to accomplish 3D navigation.
+  The nodes needed for navigation are map_server, octomap_server, amcl, and move_base.-->
+
+  <!-- Map name -->
+  <arg name="map" default="map"/>
+  <!-- Move_base configuration folder. -->
+  <arg name="move_base_config" value="3d" />
+
+  <!-- Robot sensor topic mapping-->
+  <arg name="laser_topic" default="scan"/>
+  <arg name="pointcloud_topic" default="/camera/depth/color/points"/>
+  <arg name="cmd_vel_topic" default="/stretch_diff_drive_controller/cmd_vel"/>
+  <arg name="odom_topic" default="/stretch_diff_drive_controller/odom"/>
+  <arg name="odom_link" default="odom"/>
+  <arg name="base_link" default="base_link"/>
+
+  <!-- Launch Map Server -->
+  <include file="$(find stretch_navigation)/launch/navigation/map_server.launch">
+    <arg name="map_name" value="$(find stretch_navigation)/resources/static_maps/2d/$(arg map).yaml" />
+  </include>
+
+  <!-- Launch positive and negative Octomap Server -->
+  <include file="$(find stretch_navigation)/launch/navigation/octomap_server.launch">
+    <arg name="map" value="$(find stretch_navigation)/resources/static_maps/3d/$(arg map).bt" />
+    <arg name="base_link" value="$(arg base_link)" />
+  </include>
+
+  <!--- Launch AMCL (Laser Localization) -->
+    <node pkg="amcl" type="amcl" name="amcl" output="screen">
+      <rosparam file="$(find stretch_navigation)/resources/config/amcl.yaml" command="load" />
+    </node>
+
+
+  <!-- Launch move_base -->
+  <!-- Note that you will also need to change the relevant .yaml files (or make new copies)
+       if you wish to use another namespace/robot_name. -->
+  <include file="$(find stretch_navigation)/launch/navigation/move_base.launch">
+      <arg name="config" value="$(arg move_base_config)" />
+      <arg name="cmd_vel_topic" value="$(arg cmd_vel_topic)" />
+      <arg name="odom_topic" value="$(arg odom_topic)" />
+  </include>
+
+  <node type="rviz" name="rviz" pkg="rviz" args="-d $(find stretch_navigation)/rviz/octomap.rviz" />
+
+</launch>

vz_stretch_navigation/launch/navigation_gazebo.launch

@@ -0,0 +1,54 @@
+<launch>
+
+  <arg name="map_yaml"                                                   doc="filepath to previously captured map (required)" />
+  <arg name="rviz"                   default="true"                      doc="whether to show Rviz" />
+  <arg name="gazebo_world"           default="worlds/willowgarage.world" doc="the environment within which Stretch is loaded in Gazebo" />
+  <arg name="gazebo_gpu_lidar"       default="false"                     doc="whether to compute lidar with hardware acceleration (requires GPU)" />
+  <arg name="gazebo_visualize_lidar" default="false"                     doc="whether to visualize planar lidar within Gazebo" />
+
+  <!-- GAZEBO SIMULATION -->
+  <include file="$(find stretch_gazebo)/launch/gazebo.launch">
+    <arg name="world" value="$(arg gazebo_world)" />
+    <arg name="visualize_lidar" value="$(arg gazebo_visualize_lidar)" />
+    <arg name="gpu_lidar" value="$(arg gazebo_gpu_lidar)" />
+  </include>
+
+  <!-- MAP SERVER -->
+  <node name="map_server" pkg="map_server" type="map_server" args="$(arg map_yaml)" />
+
+  <!-- LOCALIZATION -->
+  <include file="$(find amcl)/examples/amcl_diff.launch" />
+
+  <!-- NAVIGATION -->
+  <node pkg="move_base" type="move_base" respawn="false" name="move_base" output="screen">
+    <!-- The common_costmap_params_gazebo.yaml sends laser data on the /scan topic. Physical robot uses /scan_filtered -->
+    <rosparam file="$(find vz_stretch_navigation)/config/common_costmap_params_gazebo.yaml" command="load" ns="global_costmap" />
+    <rosparam file="$(find vz_stretch_navigation)/config/common_costmap_params_gazebo.yaml" command="load" ns="local_costmap" />
+    
+    <rosparam file="$(find vz_stretch_navigation)/config/local_costmap_params.yaml" command="load" />
+    <rosparam file="$(find vz_stretch_navigation)/config/global_costmap_params_withmap.yaml" command="load" />
+    
+    <!-- Only in this file we are loading teb_local_planner_params.yaml instead of base_local_planner_params.yaml -->
+    <!-- Eventually we should load teb params from the base_local_planner_params.yaml file -->
+    <!-- <rosparam file="$(find vz_stretch_navigation)/config/base_local_planner_params.yaml" command="load" /> -->
+    <rosparam file="$(find vz_stretch_navigation)/config/teb_local_planner_params_gazebo.yaml" command="load" />
+    
+    <remap from="/cmd_vel" to="/stretch_diff_drive_controller/cmd_vel" />
+
+    <!-- Switch base local planners-->
+    <!-- Uncommenting both the following uses the simplest planner. 
+         The bottom two are long term solutions which need tuning. 
+         So they will not work well as of now -->
+
+         
+    <!-- <param name="base_local_planner" value="dwa_local_planner/DWAPlannerROS" /> -->
+    <param name="base_local_planner" value="teb_local_planner/TebLocalPlannerROS" />
+    <param name="controller_frequency" value="10.0" />
+
+
+  </node>
+
+  <!-- VISUALIZE -->
+  <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
+
+</launch>

vz_stretch_navigation/launch/navigation_rviz.launch

@@ -0,0 +1,4 @@
+<launch>
+    <arg name="rviz" default="true" doc="whether to show Rviz" />
+    <node name="rviz" pkg="rviz" type="rviz" output="log" args="-d $(find vz_stretch_navigation)/rviz/navigation.rviz" if="$(arg rviz)" />
+</launch>

vz_stretch_navigation/package.xml

@@ -0,0 +1,74 @@
+<?xml version="1.0"?>
+<package format="2">
+  <name>vz_stretch_navigation</name>
+  <version>0.0.0</version>
+  <description>The vz_stretch_navigation package</description>
+
+  <!-- One maintainer tag required, multiple allowed, one person per tag -->
+  <!-- Example:  -->
+  <!-- <maintainer email="jane.doe@example.com">Jane Doe</maintainer> -->
+  <maintainer email="trivedi.ana@northeastern.edu">Ananya Trivedi</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>BSD</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/vz_stretch_navigation</url> -->
+
+
+  <!-- Author tags are optional, multiple are allowed, one per tag -->
+  <!-- Authors do not have to be maintainers, but could be -->
+  <!-- Example: -->
+  <!-- <author email="jane.doe@example.com">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>roscpp</build_depend>
+  <build_depend>rospy</build_depend>
+  <build_depend>std_msgs</build_depend>
+  <build_export_depend>roscpp</build_export_depend>
+  <build_export_depend>rospy</build_export_depend>
+  <build_export_depend>std_msgs</build_export_depend>
+  <exec_depend>roscpp</exec_depend>
+  <exec_depend>rospy</exec_depend>
+
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>move_base</exec_depend>
+  <exec_depend>amcl</exec_depend>
+  <exec_depend>map_server</exec_depend>
+  <exec_depend>gmapping</exec_depend>
+  <exec_depend>stretch_core</exec_depend>
+
+
+  <!-- The export tag contains other, unspecified, tags -->
+  <export>
+    <!-- Other tools can request additional information be placed here -->
+
+  </export>
+</package>

vz_stretch_navigation/rviz/mapping.rviz

@@ -0,0 +1,445 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Odometry1/Shape1
+        - /OccupancyGrid1
+      Splitter Ratio: 0.5
+    Tree Height: 539
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 4
+      Size (m): 0.004000000189989805
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 10
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 237; 212; 0
+        Head Length: 0.03999999910593033
+        Head Radius: 0.05000000074505806
+        Shaft Length: 0.15000000596046448
+        Shaft Radius: 0.019999999552965164
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Class: octomap_rviz_plugin/OccupancyGrid
+      Enabled: true
+      Max. Height Display: inf
+      Max. Octree Depth: 16
+      Min. Height Display: -inf
+      Name: OccupancyGrid
+      Octomap Topic: /octomap_full
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: odom
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 5.0032243728637695
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: 0.2743038535118103
+        Y: 0.44053205847740173
+        Z: 0.8585693836212158
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 3.2263076305389404
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 836
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd0000000400000000000001b5000002a6fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000002a6000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000006400000003efc0100000002fb0000000800540069006d0065010000000000000640000002eb00fffffffb0000000800540069006d0065010000000000000450000000000000000000000485000002a600000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1600
+  X: 0
+  Y: 27

vz_stretch_navigation/rviz/navigation.rviz

@@ -0,0 +1,544 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Odometry1/Shape1
+        - /Path to Goal1
+        - /Path1
+      Splitter Ratio: 0.6117647290229797
+    Tree Height: 839
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: Filtered Laser Scan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Captured Laser Scan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: Filtered Laser Scan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan_filtered
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 30
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 237; 212; 0
+        Head Length: 0.03999999910593033
+        Head Radius: 0.05000000074505806
+        Shaft Length: 0.10000000149011612
+        Shaft Radius: 0.019999999552965164
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Arrow Length: 0.10000000149011612
+      Axes Length: 0.30000001192092896
+      Axes Radius: 0.009999999776482582
+      Class: rviz/PoseArray
+      Color: 252; 175; 62
+      Enabled: true
+      Head Length: 0.07000000029802322
+      Head Radius: 0.029999999329447746
+      Name: AMCL Particles
+      Queue Size: 10
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.009999999776482582
+      Shape: Arrow (Flat)
+      Topic: /particlecloud
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Global Costmap
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: true
+      Enabled: true
+      Name: Local Costmap
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 252; 233; 79
+      Enabled: true
+      Name: Local Footprint
+      Queue Size: 10
+      Topic: /move_base/local_costmap/footprint
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Axes Length: 1
+      Axes Radius: 0.10000000149011612
+      Class: rviz/Pose
+      Color: 138; 226; 52
+      Enabled: true
+      Head Length: 0.07000000029802322
+      Head Radius: 0.10000000149011612
+      Name: Goal Pose
+      Queue Size: 10
+      Shaft Length: 0.23000000417232513
+      Shaft Radius: 0.05000000074505806
+      Shape: Arrow
+      Topic: /move_base_simple/goal
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.05000000074505806
+      Head Length: 0.019999999552965164
+      Length: 0.05000000074505806
+      Line Style: Billboards
+      Line Width: 0.029999999329447746
+      Name: Path to Goal
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 114; 159; 207
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.009999999776482582
+      Shaft Diameter: 0.03999999910593033
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 10
+      Class: rviz/Path
+      Color: 239; 41; 41
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 17.69406509399414
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -0.35903388261795044
+        Y: 0.7160218954086304
+        Z: 0.5280526876449585
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 3.1481218338012695
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1136
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000003d2fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003d000003d2000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000007380000003efc0100000002fb0000000800540069006d0065010000000000000738000002eb00fffffffb0000000800540069006d00650100000000000004500000000000000000000005dc000003d200000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1848
+  X: 72
+  Y: 27

vz_stretch_navigation/rviz/octomap.rviz

@@ -0,0 +1,405 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /OccupancyMap1
+        - /Map1
+        - /Path1
+        - /Path2
+      Splitter Ratio: 0.5
+    Tree Height: 1082
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: true
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: octomap_rviz_plugin/OccupancyMap
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Max. Octree Depth: 16
+      Name: OccupancyMap
+      Octomap Binary Topic: /octomap_full
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/local_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/local_plan
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 24.759632110595703
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -4.327556133270264
+        Y: -6.250387668609619
+        Z: -5.1258134841918945
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.785398006439209
+      Target Frame: <Fixed Frame>
+      Yaw: 0.785398006439209
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004c4000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000004c4000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e10000019700000003000005000000003efc0100000002fb0000000800540069006d00650100000000000005000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000028f000004c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 1280
+  X: 0
+  Y: 27

vz_stretch_navigation/rviz/octomap_mapper.rviz

@@ -0,0 +1,447 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Odometry1/Shape1
+        - /OccupancyGrid1
+      Splitter Ratio: 0.4206896424293518
+    Tree Height: 1109
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: LaserScan
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 0.5
+      Cell Size: 1
+      Class: rviz/Grid
+      Color: 160; 160; 164
+      Enabled: false
+      Line Style:
+        Line Width: 0.029999999329447746
+        Value: Lines
+      Name: Grid
+      Normal Cell Count: 0
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Plane: XY
+      Plane Cell Count: 10
+      Reference Frame: <Fixed Frame>
+      Value: false
+    - Alpha: 0.5
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/LaserScan
+      Color: 255; 255; 255
+      Color Transformer: Intensity
+      Decay Time: 0
+      Enabled: true
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: LaserScan
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /scan
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rviz/PointCloud2
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Decay Time: 0
+      Enabled: false
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: PointCloud2
+      Position Transformer: XYZ
+      Queue Size: 10
+      Selectable: true
+      Size (Pixels): 4
+      Size (m): 0.004000000189989805
+      Style: Points
+      Topic: /camera/depth/color/points
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: false
+    - Angle Tolerance: 0.10000000149011612
+      Class: rviz/Odometry
+      Covariance:
+        Orientation:
+          Alpha: 0.5
+          Color: 255; 255; 127
+          Color Style: Unique
+          Frame: Local
+          Offset: 1
+          Scale: 1
+          Value: true
+        Position:
+          Alpha: 0.30000001192092896
+          Color: 204; 51; 204
+          Scale: 1
+          Value: true
+        Value: true
+      Enabled: true
+      Keep: 10
+      Name: Odometry
+      Position Tolerance: 0.10000000149011612
+      Queue Size: 10
+      Shape:
+        Alpha: 1
+        Axes Length: 1
+        Axes Radius: 0.10000000149011612
+        Color: 237; 212; 0
+        Head Length: 0.03999999910593033
+        Head Radius: 0.05000000074505806
+        Shaft Length: 0.15000000596046448
+        Shaft Radius: 0.019999999552965164
+        Value: Arrow
+      Topic: /odom
+      Unreliable: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Class: octomap_rviz_plugin/OccupancyGrid
+      Enabled: true
+      Max. Height Display: inf
+      Max. Octree Depth: 16
+      Min. Height Display: -inf
+      Name: OccupancyGrid
+      Octomap Topic: /octomap_full
+      Queue Size: 5
+      Value: true
+      Voxel Alpha: 1
+      Voxel Coloring: Z-Axis
+      Voxel Rendering: Occupied Voxels
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 11.191774368286133
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -5.315392971038818
+        Y: -3.4429707527160645
+        Z: -0.27000176906585693
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.0197970867156982
+      Target Frame: <Fixed Frame>
+      Yaw: 3.8762691020965576
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1403
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd00000004000000000000025f000004dffc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004df000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f00000363fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073000000003d00000363000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000079b000004df00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 2560
+  Y: 0

vz_stretch_navigation/rviz/rtab_mapping.rviz

@@ -0,0 +1,396 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+      Splitter Ratio: 0.5
+    Tree Height: 787
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+      - /Current View1/Focal Point1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: MapCloud
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 1
+      Class: rtabmap_ros/MapGraph
+      Enabled: true
+      Global loop closure: 255; 0; 0
+      Landmark: 0; 128; 0
+      Local loop closure: 255; 255; 0
+      Merged neighbor: 255; 170; 0
+      Name: MapGraph
+      Neighbor: 0; 0; 255
+      Queue Size: 10
+      Topic: /rtabmap/mapGraph
+      Unreliable: false
+      User: 255; 0; 0
+      Value: true
+      Virtual: 255; 0; 255
+    - Alpha: 1
+      Autocompute Intensity Bounds: true
+      Autocompute Value Bounds:
+        Max Value: 10
+        Min Value: -10
+        Value: true
+      Axis: Z
+      Channel Name: intensity
+      Class: rtabmap_ros/MapCloud
+      Cloud decimation: 4
+      Cloud from scan: false
+      Cloud max depth (m): 4
+      Cloud min depth (m): 0
+      Cloud voxel size (m): 0.009999999776482582
+      Color: 255; 255; 255
+      Color Transformer: RGB8
+      Download graph: false
+      Download map: false
+      Enabled: true
+      Filter ceiling (m): 0
+      Filter floor (m): 0
+      Invert Rainbow: false
+      Max Color: 255; 255; 255
+      Min Color: 0; 0; 0
+      Name: MapCloud
+      Node filtering angle (degrees): 30
+      Node filtering radius (m): 0
+      Position Transformer: XYZ
+      Queue Size: 10
+      Size (Pixels): 3
+      Size (m): 0.009999999776482582
+      Style: Flat Squares
+      Topic: /rtabmap/mapData
+      Unreliable: false
+      Use Fixed Frame: true
+      Use rainbow: true
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: costmap
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /move_base/global_costmap/costmap
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Class: rviz/Polygon
+      Color: 25; 255; 0
+      Enabled: true
+      Name: Polygon
+      Queue Size: 10
+      Topic: /move_base/global_costmap/footprint
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rtabmap_ros/OrbitOriented
+      Distance: 17.96693229675293
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -3.853421926498413
+        Y: 0.7817280888557434
+        Z: 0.4164792597293854
+      Focal Shape Fixed Size: false
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 0.7947970628738403
+      Target Frame: <Fixed Frame>
+      Yaw: 4.218579292297363
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: true
+  Height: 1016
+  Hide Left Dock: true
+  Hide Right Dock: true
+  QMainWindow State: 000000ff00000000fd0000000400000000000001560000039efc0200000007fb0000001200530065006c0065006300740069006f006e000000003d0000009b0000005c00fffffffb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073000000003d0000039e000000c900fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f0000039efc0200000003fb0000001e0054006f006f006c002000500072006f0070006500720074006900650073000000003d000000810000005c00fffffffb0000000a0056006900650077007300000000c400000317000000a400fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e100000197000000030000078000000134fc0100000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730000000000000007800000000000000000fb0000000800540069006d0065000000000000000780000004f300fffffffb0000000800540069006d00650100000000000004500000000000000000000003c00000039e00000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: true
+  Views:
+    collapsed: true
+  Width: 960
+  X: 0
+  Y: 27

vz_stretch_navigation/rviz/rtab_navigation.rviz

@@ -0,0 +1,374 @@
+Panels:
+  - Class: rviz/Displays
+    Help Height: 78
+    Name: Displays
+    Property Tree Widget:
+      Expanded:
+        - /Global Options1
+        - /Status1
+        - /Map1
+        - /Path1
+      Splitter Ratio: 0.5
+    Tree Height: 1082
+  - Class: rviz/Selection
+    Name: Selection
+  - Class: rviz/Tool Properties
+    Expanded:
+      - /2D Pose Estimate1
+      - /2D Nav Goal1
+      - /Publish Point1
+    Name: Tool Properties
+    Splitter Ratio: 0.5886790156364441
+  - Class: rviz/Views
+    Expanded:
+      - /Current View1
+    Name: Views
+    Splitter Ratio: 0.5
+  - Class: rviz/Time
+    Experimental: false
+    Name: Time
+    SyncMode: 0
+    SyncSource: ""
+Preferences:
+  PromptSaveOnExit: true
+Toolbars:
+  toolButtonStyle: 2
+Visualization Manager:
+  Class: ""
+  Displays:
+    - Alpha: 1
+      Class: rviz/RobotModel
+      Collision Enabled: false
+      Enabled: true
+      Links:
+        All Links Enabled: true
+        Expand Joint Details: false
+        Expand Link Details: false
+        Expand Tree: false
+        Link Tree Style: Links in Alphabetic Order
+        base_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        camera_accel_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_accel_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_bottom_screw_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_color_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_depth_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_gyro_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra1_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_infra2_optical_frame:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        camera_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        caster_link:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        laser:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l0:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l1:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l2:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l3:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_arm_l4:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_inner_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_left_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_right_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_shoulder:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_aruco_top_wrist:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_grasp_center:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+        link_gripper:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_finger_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_left:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_gripper_fingertip_right:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_pan:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_head_tilt:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_left_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_lift:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_mast:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_right_wheel:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        link_wrist_yaw:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+        respeaker_base:
+          Alpha: 1
+          Show Axes: false
+          Show Trail: false
+          Value: true
+      Name: RobotModel
+      Robot Description: robot_description
+      TF Prefix: ""
+      Update Interval: 0
+      Value: true
+      Visual Enabled: true
+    - Alpha: 0.699999988079071
+      Class: rviz/Map
+      Color Scheme: map
+      Draw Behind: false
+      Enabled: true
+      Name: Map
+      Topic: /rtabmap/grid_prob_map
+      Unreliable: false
+      Use Timestamp: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /move_base/DWAPlannerROS/global_plan
+      Unreliable: false
+      Value: true
+    - Alpha: 1
+      Buffer Length: 1
+      Class: rviz/Path
+      Color: 25; 255; 0
+      Enabled: true
+      Head Diameter: 0.30000001192092896
+      Head Length: 0.20000000298023224
+      Length: 0.30000001192092896
+      Line Style: Lines
+      Line Width: 0.029999999329447746
+      Name: Path
+      Offset:
+        X: 0
+        Y: 0
+        Z: 0
+      Pose Color: 255; 85; 255
+      Pose Style: None
+      Queue Size: 10
+      Radius: 0.029999999329447746
+      Shaft Diameter: 0.10000000149011612
+      Shaft Length: 0.10000000149011612
+      Topic: /rtabmap/mapPath
+      Unreliable: false
+      Value: true
+  Enabled: true
+  Global Options:
+    Background Color: 48; 48; 48
+    Default Light: true
+    Fixed Frame: map
+    Frame Rate: 30
+  Name: root
+  Tools:
+    - Class: rviz/Interact
+      Hide Inactive Objects: true
+    - Class: rviz/MoveCamera
+    - Class: rviz/Select
+    - Class: rviz/FocusCamera
+    - Class: rviz/Measure
+    - Class: rviz/SetInitialPose
+      Theta std deviation: 0.2617993950843811
+      Topic: /initialpose
+      X std deviation: 0.5
+      Y std deviation: 0.5
+    - Class: rviz/SetGoal
+      Topic: /move_base_simple/goal
+    - Class: rviz/PublishPoint
+      Single click: true
+      Topic: /clicked_point
+  Value: true
+  Views:
+    Current:
+      Class: rviz/Orbit
+      Distance: 24.471881866455078
+      Enable Stereo Rendering:
+        Stereo Eye Separation: 0.05999999865889549
+        Stereo Focal Distance: 1
+        Swap Stereo Eyes: false
+        Value: false
+      Field of View: 0.7853981852531433
+      Focal Point:
+        X: -2.2477076053619385
+        Y: -1.80465567111969
+        Z: -6.365629196166992
+      Focal Shape Fixed Size: true
+      Focal Shape Size: 0.05000000074505806
+      Invert Z Axis: false
+      Name: Current View
+      Near Clip Distance: 0.009999999776482582
+      Pitch: 1.5697963237762451
+      Target Frame: <Fixed Frame>
+      Yaw: 1.2835423946380615
+    Saved: ~
+Window Geometry:
+  Displays:
+    collapsed: false
+  Height: 1376
+  Hide Left Dock: false
+  Hide Right Dock: false
+  QMainWindow State: 000000ff00000000fd000000040000000000000156000004c4fc0200000008fb0000001200530065006c0065006300740069006f006e00000001e10000009b0000005c00fffffffb0000001e0054006f006f006c002000500072006f007000650072007400690065007302000001ed000001df00000185000000a3fb000000120056006900650077007300200054006f006f02000001df000002110000018500000122fb000000200054006f006f006c002000500072006f0070006500720074006900650073003203000002880000011d000002210000017afb000000100044006900730070006c006100790073010000003c000004c4000000c800fffffffb0000002000730065006c0065006300740069006f006e00200062007500660066006500720200000138000000aa0000023a00000294fb00000014005700690064006500530074006500720065006f02000000e6000000d2000003ee0000030bfb0000000c004b0069006e0065006300740200000186000001060000030c00000261000000010000010f000004c4fc0200000003fb0000001e0054006f006f006c002000500072006f00700065007200740069006500730100000041000000780000000000000000fb0000000a00560069006500770073010000003c000004c4000000a200fffffffb0000001200530065006c0065006300740069006f006e010000025a000000b200000000000000000000000200000490000000a9fc0100000001fb0000000a00560069006500770073030000004e00000080000002e1000001970000000300000a000000003efc0100000002fb0000000800540069006d0065010000000000000a000000029800fffffffb0000000800540069006d006501000000000000045000000000000000000000078f000004c400000004000000040000000800000008fc0000000100000002000000010000000a0054006f006f006c00730100000000ffffffff0000000000000000
+  Selection:
+    collapsed: false
+  Time:
+    collapsed: false
+  Tool Properties:
+    collapsed: false
+  Views:
+    collapsed: false
+  Width: 2560
+  X: 0
+  Y: 27

vz_stretch_navigation/scripts/simple_navigation_goal_client.py

@@ -0,0 +1,67 @@
+#!/usr/bin/env python
+# license removed for brevity
+
+import rospy
+
+# Brings in the SimpleActionClient
+import actionlib
+# Brings in the .action file and messages used by the move base action
+from move_base_msgs.msg import MoveBaseAction, MoveBaseGoal
+# Gazebo messages
+from gazebo_msgs.msg import ModelStates
+
+# wrap all stuff below in a class later. global variables for now.sorry
+goal_dict = {"x" : 0.0, "y": 0.0 , "orientation" : 0.0}
+
+# A lot can be done here. Later 
+def generate_goal_pose(data):
+    
+    person_of_interest_idx = data.name.index('unit_box')
+    # return a goal that is 5,5 in front of the object
+    goal_dict["x"] = data.pose[person_of_interest_idx].position.x - 8.0
+    goal_dict["y"] = data.pose[person_of_interest_idx].position.y - 8.0
+    return goal_dict
+
+def movebase_client():
+
+   # Create an action client called "move_base" with action definition file "MoveBaseAction"
+    client = actionlib.SimpleActionClient('move_base',MoveBaseAction)
+
+   # Waits until the action server has started up and started listening for goals.
+    client.wait_for_server()
+
+   # Creates a new goal with the MoveBaseGoal constructor
+    goal = MoveBaseGoal()
+    goal.target_pose.header.frame_id = "map"
+    goal.target_pose.header.stamp = rospy.Time.now()
+
+   # Generate a goal pose that gets us within 5m x and 5m y of the unit_block
+    # goal.target_pose.pose.position.x = 0.5
+    goal.target_pose.pose.position.x = goal_dict["x"]
+    goal.target_pose.pose.position.x = goal_dict["y"]
+   # No rotation of the mobile base frame w.r.t. map frame
+    goal.target_pose.pose.orientation.w = 1.0
+
+   # Sends the goal to the action server.
+    client.send_goal(goal)
+   # Waits for the server to finish performing the action.
+    wait = client.wait_for_result()
+   # If the result doesn't arrive, assume the Server is not available
+    if not wait:
+        rospy.logerr("Action server not available!")
+        rospy.signal_shutdown("Action server not available!")
+    else:
+    # Result of executing the action
+        return client.get_result()   
+
+# If the python node is executed as main process (sourced directly)
+if __name__ == '__main__':
+    try:
+       # Initializes a rospy node to let the SimpleActionClient publish and subscribe
+        rospy.init_node('movebase_client_py')
+        rospy.Subscriber("gazebo/model_states",ModelStates,generate_goal_pose)
+        result = movebase_client()
+        if result:
+            rospy.loginfo("Goal execution done!")
+    except rospy.ROSInterruptException:
+        rospy.loginfo("Navigation test finished.")