ROS wrappers for thermal and rgbd, Also Human State Estimation ROSification rev 0 works

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,3 @@
+<launch>
+    <node pkg="vz_human_state_estimation" type="ros_interface.py" name="human_state_estimation" />
+</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,208 @@
+#! /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
+
+
+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()
+
+    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,98 @@
+#! /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
+        self.MDA = mda.ConnectToROS()
+
+        # set up CV Bridge
+        self.bridge = CvBridge()
+
+        #Image subscribers
+        rgbd_sub    = message_filters.Subscriber("/D435i_raw",Image)
+        thermal_sub = message_filters.Subscriber("/flir_3_5_near_realsense_raw",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)
+    
+    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)
+        self.cv_bounding_boxed_img = self.MDA.processData(self.cv_rgbd_img,self.cv_thermal_img)
+
+    def run(self):
+        while not rospy.is_shutdown():
+            if(self.cv_rgbd_img is None or self.cv_thermal_img is None or self.cv_bounding_boxed_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.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_ros_wrappers/CMakeLists.txt

@@ -0,0 +1,209 @@
+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
+)
+
+## 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_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}
+)
+
+## 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(${PROJECT_NAME}_node src/vz_ros_wrappers_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_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,4 @@
+<launch>
+      <node pkg="vz_ros_wrappers" type="grab_D435i.py" name="D435i" />
+      <node pkg="vz_ros_wrappers" type="grab_thermal.py" name="FLIRLepton" />
+  </launch>

vz_ros_wrappers/package.xml

@@ -0,0 +1,77 @@
+<?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,90 @@
+#! /usr/bin/python3
+'''
+Derived from this by 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
+        # setup subscribed topics
+        # initialize ros/cv2 bridge
+        
+        # Note: when testing with your own video, replace with local file
+        self.D435i_cap = cv2.VideoCapture('/home/ananya/Downloads/2022-02-09-16-20-58(2).mp4')
+
+        if not self.D435i_cap.isOpened():
+            raise(Exception,'Unable to open video stream')
+        self.bridge = CvBridge()
+        self.D435i_pub = rospy.Publisher('/D435i_raw', 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.thermal_image is None:
+        if self.D435i_image is None:
+            return
+        try:
+            # Show images
+            cv2.imshow('D435i',cv2.resize(self.D435i_image,(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 : 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,97 @@
+#! /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
+
+        # 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('/home/ananya/Downloads/Thermal-3(2).mp4')
+        # self.thermal_cap = cv2.VideoCapture('/dev/video6')
+        
+        if not self.thermal_cap.isOpened():
+            raise(Exception,'Unable to open video stream')
+        self.bridge = CvBridge()
+        self.thermal_pub = rospy.Publisher('/flir_3_5_near_realsense_raw', 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
+            cv2.imshow('Lepton',cv2.resize(self.thermal_image,(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 : 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()