GPU-accelerated obstacle detection

.gitattributes

@@ -1,9 +1,18 @@
 text=auto
+# Image files
 *.png filter=lfs diff=lfs merge=lfs -text
+# Model files
 *.dae filter=lfs diff=lfs merge=lfs -text
-*.blend filter=lfs diff=lfs merge=lfs -text
 *.stl filter=lfs diff=lfs merge=lfs -text
+*.blend filter=lfs diff=lfs merge=lfs -text
+# Neural network files
+*.pt filter=lfs diff=lfs merge=lfs -text
+*.onnx filter=lfs diff=lfs merge=lfs -text
+*.engine filter=lfs diff=lfs merge=lfs -text
+# Model files
 *.glb filter=lfs diff=lfs merge=lfs -text
 *.fbx filter=lfs diff=lfs merge=lfs -text
+# Packages
 *.deb filter=lfs diff=lfs merge=lfs -text
+# Program files
 ansible/roles/esw/files/bin/fdcanusb_daemon filter=lfs diff=lfs merge=lfs -text

CMakeLists.txt

@@ -61,6 +61,7 @@ set(MROVER_MESSAGE_DEPENDENCIES
 
 set(MROVER_PARAMETERS
         config/DetectorParams.cfg
+        config/ObjectDetectorParams.cfg
 )
 
 set(MROVER_CMAKE_INCLUDES
@@ -172,6 +173,16 @@ if (CUDA_FOUND)
 endif ()
 
 if (ZED_FOUND)
+    mrover_add_nodelet(object_detector src/perception/object_detector/*.c* src/perception/object_detector src/perception/object_detector/pch.hpp)
+    mrover_nodelet_link_libraries(object_detector PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb)
+    mrover_nodelet_defines(object_detector
+            __CUDA_INCLUDE_COMPILER_INTERNAL_HEADERS__ # Eigen includes some files it should not, ignore
+    )
+    # TODO(john): Update object detector to use new API
+    target_compile_options(object_detector_nodelet PRIVATE -Wno-deprecated-declarations)
+    # Temporary
+    mrover_nodelet_link_libraries(object_detector PRIVATE opencv_imgcodecs opencv_highgui)
+
     mrover_add_nodelet(zed src/perception/zed_wrapper/*.c* src/perception/zed_wrapper src/perception/zed_wrapper/pch.hpp)
     mrover_nodelet_include_directories(zed ${ZED_INCLUDE_DIRS} ${CUDA_INCLUDE_DIRS})
     mrover_nodelet_link_libraries(zed ${ZED_LIBRARIES} ${SPECIAL_OS_LIBS} lie)

config/ObjectDetectorParams.cfg

@@ -0,0 +1,10 @@
+#!/usr/bin/env python3
+PACKAGE = "mrover"
+
+from dynamic_reconfigure.parameter_generator_catkin import *
+
+gen = ParameterGenerator()
+
+# TODO(percep/obj-detector): add parameters
+
+exit(gen.generate(PACKAGE, "object_detector", "ObjectDetectorParams"))

config/simulator/simulator.yaml

@@ -2,7 +2,7 @@ objects:
   - type: urdf
     name: rover
     uri: package://mrover/urdf/rover/rover.urdf.xacro
-    translation: [ 0, 0, 0.5 ]
+    translation: [0, 0, 0.5]
   - type: urdf
     name: world
     uri: package://mrover/urdf/world/world.urdf.xacro
@@ -13,28 +13,28 @@ objects:
   - type: urdf
     name: tag_1
     uri: package://mrover/urdf/world/tag_1.urdf.xacro
-    translation: [ 15, -14, 2.4 ]
+    translation: [15, -14, 2.4]
   - type: urdf
     name: hammer
     uri: package://mrover/urdf/world/hammer.urdf.xacro
-    translation: [ 8, 7, 0.6 ]
+    translation: [5, 2, 0.7]
   - type: urdf
     name: bottle
     uri: package://mrover/urdf/world/bottle.urdf.xacro
-    translation: [ -6.25, 10.5, 0.3 ]
+    translation: [-6.25, 10.5, 0.3]
   - type: urdf
     name: rock1
     uri: package://mrover/urdf/world/rock.urdf.xacro
-    translation: [ 2, -2, 0.1 ]
+    translation: [2, -2, 0.1]
   - type: urdf
     name: rock2
     uri: package://mrover/urdf/world/rock.urdf.xacro
-    translation: [ 7, -6, 0.2 ]
+    translation: [7, -6, 0.2]
   - type: urdf
     name: rock3
     uri: package://mrover/urdf/world/rock.urdf.xacro
-    translation: [ 8, -10, 1 ]
+    translation: [8, -10, 1]
   - type: urdf
     name: rock4
     uri: package://mrover/urdf/world/rock.urdf.xacro
-    translation: [ 9, -2, 0.2 ]
+    translation: [9, -2, 0.2]

launch/perception.launch

@@ -1,8 +1,17 @@
 <launch>
     <arg name="run_tag_detector" default="true" />
+    <arg name="run_object_detector" default="true" />
 
     <!-- Nodelet to detect AR tags and publish them to the TF tree -->
     <node if="$(arg run_tag_detector)"
-        pkg="nodelet" type="nodelet" name="tag_detector"
+        pkg="nodelet" type="nodelet" name="zed_tag_detector"
         args="load mrover/TagDetectorNodelet nodelet_manager" output="screen" />
-</launch>
+
+    <node if="$(arg run_tag_detector)"
+        pkg="nodelet" type="nodelet" name="long_range_tag_detector"
+        args="load mrover/LongRangeTagDetectorNodelet nodelet_manager" output="screen" />
+
+    <node if="$(arg run_object_detector)"
+        pkg="nodelet" type="nodelet" name="object_detector" respawn="true"
+        args="load mrover/ObjectDetectorNodelet perception_nodelet_manager" output="screen" />
+</launch>

launch/zed.launch

@@ -6,6 +6,7 @@
     <arg name="use_builtin_visual_odom" default="false" />
 
     <rosparam command="load" file="$(find mrover)/config/perception.yaml" />
+
     <node pkg="nodelet" type="nodelet" name="zed_nodelet" respawn="true"
         args="load mrover/ZedNodelet nodelet_manager" output="screen">
         <param name="use_builtin_visual_odom" value="$(arg use_builtin_visual_odom)" />

launch/zed_test.launch

@@ -4,14 +4,21 @@
       <arg name="run_rqt_tf_tree" default="true" />
 
       <arg name="run_tag_detector" default="true" />
+      <arg name="run_object_detector" default="true" />
       <arg name="use_builtin_visual_odom" default="false" />
 
+      <arg name="obj_increment_weight" default="3" />
+      <arg name="obj_decrement_weight" default="1" />
+      <arg name="obj_hitcount_threshold" default="50" />
+      <arg name="obj_hitcount_max" default="500" />
+
       <include file="$(find mrover)/launch/zed.launch">
             <arg name="use_builtin_visual_odom" value="$(arg use_builtin_visual_odom)" />
       </include>
 
       <include file="$(find mrover)/launch/perception.launch">
             <arg name="run_tag_detector" value="$(arg run_tag_detector)" />
+            <arg name="run_object_detector" value="$(arg run_object_detector)" />
       </include>
 
       <group if="$(arg run_rviz)">
@@ -30,4 +37,4 @@
 
       <node pkg="tf" type="static_transform_publisher" name="base_link_publisher"
             args="1 0 0 0 0 0 map base_link 100" />
-</launch>
+</launch>

package.xml

@@ -18,7 +18,6 @@
   <!-- Example: -->
   <!-- <author email="[email protected]">Jane Doe</author> -->
 
-
   <!-- The *depend tags are used to specify dependencies -->
   <!-- Dependencies can be catkin packages or system dependencies -->
   <!-- Examples: -->
@@ -78,9 +77,10 @@
 
   <!-- The export tag contains other, unspecified, tags -->
   <export>
-    <nodelet plugin="${prefix}/plugins/tag_detector_plugin.xml" />
-    <nodelet plugin="${prefix}/plugins/zed_plugin.xml" />
-    <nodelet plugin="${prefix}/plugins/can.xml" />
-    <nodelet plugin="${prefix}/plugins/simulator.xml" />
+    <nodelet plugin="${prefix}/plugins/tag_detector_plugin.xml"/>
+    <nodelet plugin="${prefix}/plugins/zed_plugin.xml"/>
+    <nodelet plugin="${prefix}/plugins/can.xml"/>
+    <nodelet plugin="${prefix}/plugins/simulator.xml"/>
+    <nodelet plugin="${prefix}/plugins/object_detector_plugin.xml"/>
   </export>
-</package>
+</package>

plugins/object_detector_plugin.xml

@@ -0,0 +1,6 @@
+<library path="lib/libobject_detector_nodelet">
+  <class name="mrover/ObjectDetectorNodelet"
+         type="mrover::ObjectDetectorNodelet"
+         base_class_type="nodelet::Nodelet">
+  </class>
+</library>

src/perception/image_capture/image_capturepy.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+
+from pynput import keyboard
+
+import rospkg
+
+import os
+
+# python linear algebra library
+import numpy as np
+
+# library for interacting with ROS and TF tree
+import rospy
+
+import cv2
+
+import datetime
+
+
+# ROS message types we need to use
+from sensor_msgs.msg import Image
+
+
+def on_press(key):
+    if key == keyboard.Key.enter:
+        msg = rospy.wait_for_message("/camera/left/image", Image, timeout=5)
+        data = np.empty(msg.height * msg.width * 4, dtype=np.uint8)
+        for x in range(msg.height * msg.width * 4):
+            data[x] = msg.data[x]
+
+        image = np.reshape(data, [msg.height, msg.width, 4])
+        unique_id = "{date:%Y-%m-%d_%H:%M:%S}".format(date=datetime.datetime.now())
+        rospack = rospkg.RosPack()
+        pkgPath = rospack.get_path("mrover")
+        imagePath = pkgPath + f"/data/Images"
+        if not os.path.exists(imagePath):
+            os.mkdir(imagePath)
+
+        print(cv2.imwrite(imagePath + f"image_{unique_id}.jpg", image))
+
+
+def on_release(key):
+    while False:
+        continue
+
+
+class image_capturepy:
+    def __init__(self):
+        while False:
+            continue
+
+
+def main():
+    # initialize the node
+    rospy.init_node("image_capturepy")
+
+    listener = keyboard.Listener(on_press=on_press, on_release=on_release)
+    listener.start()
+
+    # let the callback functions run asynchronously in the background
+    rospy.spin()
+
+
+if __name__ == "__main__":
+    main()