Add the ZED Wrapper and Object Detector (#3)

* starter files * starter code * finding zed sdk * building zed sdk * y no build * gets paramaters * gets more aprameters * why no callback * variants :))))) * logic error * more params * most init done * no build :( * almost builds * builds :))))) * wat svo? * builds * AHHHHHHHH POINTERS AHHHHHHHHH * STD MOVE FTW * undefined symbols :( * undefined symbols :( * Builds * Runs * Better includes * Works * 62 Hz * begin porting voer obj detect * more errors * learning lib builds * brah * bruhhhh * works * better type safety * got rid of compiler warnings * starter files * starter code * finding zed sdk * building zed sdk * y no build * gets paramaters * gets more aprameters * why no callback * variants :))))) * logic error * more params * most init done * no build :( * almost builds * builds :))))) * wat svo? * builds * AHHHHHHHH POINTERS AHHHHHHHHH * STD MOVE FTW * undefined symbols :( * undefined symbols :( * Builds * Runs * Better includes * Works * 62 Hz * begin porting voer obj detect * more errors * learning lib builds * brah * bruhhhh * works * better type safety * got rid of compiler warnings * Update zed_wrapper.bridge.cpp * Updated CUDA * Updated CUDA * Update zed_wrapper.bridge.cpp * CLANG FORMAT IS CRACCCCCCKKKKKKED bro :)))))))))))) * CLANG FORMAT IS CRACCCCCCKKKKKKED bro :)))))))))))) * CLANG FORMAT IS CRACCCCCCKKKKKKED bro :)))))))))))) * CLANG FORMAT IS CRACCCCCCKKKKKKED bro :)))))))))))) * BLACK * builds * no engine * redundant * Ashwin Suggestions * No more engine * Updating compiler flags * Final Refactoring * od still works * zed default params * od default params * Works with new parameters * Better pathing on zed launch * Better launch file configuration * works
umrover · Sep 19, 2024 · 05833a6 · 05833a6
1 parent 6adc917
commit 05833a6
Show file tree

Hide file tree

Showing 27 changed files with 1,723 additions and 3 deletions.
diff --git a/CMakeLists.txt b/CMakeLists.txt
@@ -5,7 +5,7 @@ project(mrover VERSION 2025.0.0 LANGUAGES C CXX)
 cmake_policy(SET CMP0148 OLD)
 
 set(CMAKE_CXX_STANDARD 23)
-set(CMAKE_CXX_STANDARD_REQUIRED ON)
+
 # Generate compile_commands.json for clangd language server.
 # Can be used by VSCode, CLion, VIM, etc.
 set(CMAKE_EXPORT_COMPILE_COMMANDS ON)
@@ -210,6 +210,7 @@ ament_target_dependencies(lie rclcpp geometry_msgs tf2 tf2_ros)
 
 mrover_add_header_only_library(units units)
 mrover_add_header_only_library(loop_profiler loop_profiler)
+mrover_add_header_only_library(parameter_utils parameter_utils)
 
 # Simulator
 
@@ -226,6 +227,46 @@ target_include_directories(simulator SYSTEM PRIVATE ${BULLET_INCLUDE_DIRS} ${OPE
 
 # Perception
 
+find_package(ZED QUIET)
+find_package(CUDA QUIET)
+find_package(OpenCV REQUIRED)
+if(ZED_FOUND AND CUDA_FOUND)
+	enable_language(CUDA)
+	# CUDA Compile Options
+	add_library(cuda_compiler_flags INTERFACE)
+	target_compile_options(cuda_compiler_flags INTERFACE 
+		-Wno-pedantic 
+		-Wno-deprecated 
+		-Wno-unused-parameter 
+		-diag-suppress=815 
+		-diag-suppress=780 
+		-Wno-deprecated-copy 
+		-Wno-unused-command-line-argument 
+		-Wno-ignored-qualifiers 
+		-Wno-sometimes-uninitialized
+	)
+
+	# ZED Wrapper
+	mrover_add_node(zed perception/zed_wrapper/*.c* perception/zed_wrapper/pch.hpp)
+	target_compile_options(zed PRIVATE $<$<COMPILE_LANGUAGE:CXX>:-std=c++20>)
+	target_link_libraries(zed parameter_utils lie MANIF::manif ${CUDA_LIBRARIES} loop_profiler cuda_compiler_flags)
+	ament_target_dependencies(zed rclcpp sensor_msgs ZED CUDA tf2 tf2_ros)
+
+	# Learning Library
+    # TODO(john): Update to use the new API
+	mrover_add_library(tensorrt tensorrt/*.c* tensorrt)
+	target_compile_options(tensorrt PRIVATE -Wno-deprecated-declarations $<$<COMPILE_LANGUAGE:CUDA>:-std=c++17>)
+	target_include_directories(tensorrt PRIVATE ${CMAKE_CUDA_TOOLKIT_INCLUDE_DIRECTORIES})
+	target_link_libraries(tensorrt PRIVATE opencv_core opencv_dnn opencv_imgproc lie nvinfer nvonnxparser tbb cuda_compiler_flags)
+
+	# Object Detector
+	mrover_add_node(object_detector perception/object_detector/*.c* src/perception/object_detector/pch.hpp)
+    target_link_libraries(object_detector opencv_core opencv_dnn opencv_imgproc lie tbb tensorrt opencv_imgcodecs opencv_highgui loop_profiler parameter_utils cuda_compiler_flags)
+	ament_target_dependencies(object_detector rclcpp sensor_msgs CUDA tf2 tf2_ros)
+else()
+	message("ZED not found...")
+endif()
+
 mrover_add_node(tag_detector perception/tag_detector/*.cpp perception/tag_detector/pch.hpp)
 ament_target_dependencies(tag_detector rclcpp tf2 tf2_ros)
 target_link_libraries(tag_detector lie opencv_core opencv_aruco opencv_imgproc loop_profiler)

diff --git a/build.sh b/build.sh
@@ -5,10 +5,14 @@ set -euxo pipefail
 # Build in the colcon workspace, not the package
 pushd ../..
 
-# set C/CXX compilers
+# Set C/C++ compilers
 export CC=clang
 export CXX=clang++
 
+# Set CUDA compilers
+export CUDAHOSTCXX=g++-9
+export CUDACXX=/usr/local/cuda-12.3/bin/nvcc
+
 # TODO (ali): add build configs for debug vs release
 colcon build \
 	--cmake-args -G Ninja -W no-dev -DCMAKE_BUILD_TYPE=RelWithDebInfo \

diff --git a/config/object_detector.yaml b/config/object_detector.yaml
@@ -0,0 +1,18 @@
+
+# All units are in SI
+# ===================
+# Time:     second, hz
+# Angle:    radian
+# Distance: meter
+
+/object_detector:
+  ros__parameters:
+    camera_frame: "zed_left_camera_frame"
+    world_frame: "map"
+    increment_weight: 2
+    decrement_weight: 1
+    hitcount_threshold: 5
+    hitcount_max: 10
+    model_name: "Large-Dataset"
+    model_score_threshold: 0.75
+    model_nms_threshold: 0.5
diff --git a/config/zed.yaml b/config/zed.yaml
@@ -0,0 +1,22 @@
+# All units are in SI
+# ===================
+# Time:     second, hz
+# Angle:    radian
+# Distance: meter
+
+/zed_wrapper:
+  ros__parameters:
+    depth_confidence: 70
+    serial_number: -1
+    grab_target_fps: 60
+    texture_confidence: 100
+    image_width: 1280
+    image_height: 720
+    svo_file: ""
+    use_depth_stabilization: false
+    grab_resolution: "HD720"
+    depth_mode: "PERFORMANCE"
+    depth_maximum_distance: 12.0
+    use_builtin_visual_odom: false
+    use_pose_smoothing: true
+    use_area_memory: true
diff --git a/data/.gitignore b/data/.gitignore
@@ -0,0 +1 @@
+*.engine
diff --git a/data/Large-Dataset.onnx b/data/Large-Dataset.onnx
diff --git a/launch/perception.launch.py b/launch/perception.launch.py
@@ -0,0 +1,28 @@
+from pathlib import Path
+
+from ament_index_python import get_package_share_directory
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node, ComposableNodeContainer
+from launch_ros.descriptions import ComposableNode
+from launch.conditions import LaunchConfigurationEquals
+
+
+def generate_launch_description():
+    zed_node = Node(
+        package="mrover",
+        executable="zed",
+        name="zed_wrapper",
+        parameters=[Path(get_package_share_directory("mrover"), "config", "zed.yaml")],
+    )
+
+    object_detector_node = Node(
+        package="mrover",
+        executable="object_detector",
+        name="object_detector",
+        parameters=[Path(get_package_share_directory("mrover"), "config", "object_detector.yaml")],
+    )
+
+    return LaunchDescription([zed_node, object_detector_node])
diff --git a/parameter_utils/parameter.hpp b/parameter_utils/parameter.hpp
@@ -0,0 +1,104 @@
+#include <vector>
+#include <string>
+#include <variant>
+#include <exception>
+#include <rclcpp/rclcpp.hpp>
+
+// Overloads pattern for visit
+template<class... Ts> struct overload : Ts... { using Ts::operator()...; };
+template<class... Ts> overload(Ts...) -> overload<Ts...>;
+
+
+namespace mrover {
+	class ParameterWrapper {
+	private:
+		static inline std::shared_ptr<rclcpp::ParameterCallbackHandle> cbHande;
+
+	public:
+		rclcpp::ParameterType mType;
+
+		std::string mParamDescriptor;
+
+		std::variant<int*, std::string*, bool*, double*, float*> mData;
+
+		std::variant<int, std::string, bool, double, float> mDefaultValue;
+
+		ParameterWrapper(std::string paramDescriptor, int& variable, int defaultValue = 0) : mType{rclcpp::ParameterType::PARAMETER_INTEGER}, mParamDescriptor{std::move(paramDescriptor)}, mData{&variable}, mDefaultValue{defaultValue}{}
+
+		ParameterWrapper(std::string paramDescriptor, std::string& variable, std::string defaultValue = "") : mType{rclcpp::ParameterType::PARAMETER_STRING}, mParamDescriptor{std::move(paramDescriptor)}, mData{&variable}, mDefaultValue{std::move(defaultValue)}{}
+
+		ParameterWrapper(std::string paramDescriptor, bool& variable, bool defaultValue = false) : mType{rclcpp::ParameterType::PARAMETER_BOOL}, mParamDescriptor{std::move(paramDescriptor)}, mData{&variable}, mDefaultValue{defaultValue}{}
+
+		ParameterWrapper(std::string paramDescriptor, double& variable, double defaultValue = 0.0) : mType{rclcpp::ParameterType::PARAMETER_DOUBLE}, mParamDescriptor{std::move(paramDescriptor)}, mData{&variable}, mDefaultValue{defaultValue}{}
+
+		ParameterWrapper(std::string paramDescriptor, float& variable, float defaultValue = 0.0) : mType{rclcpp::ParameterType::PARAMETER_DOUBLE}, mParamDescriptor{std::move(paramDescriptor)}, mData{&variable}, mDefaultValue{defaultValue}{}
+
+		void visit(rclcpp::Node* node){
+			std::visit(overload{
+				[&](int* arg){
+					try{
+						*arg = static_cast<int>(node->get_parameter(mParamDescriptor).as_int());
+					}catch(rclcpp::exceptions::ParameterUninitializedException& e){
+						try{
+							*arg = std::get<int>(mDefaultValue);
+						}catch (std::bad_variant_access const& ex){
+							throw std::runtime_error("Bad Variant Access: Type not int");
+						}
+					}
+				},
+				[&](std::string* arg){
+					try{
+							*arg = node->get_parameter(mParamDescriptor).as_string();
+					}catch(rclcpp::exceptions::ParameterUninitializedException& e){
+						try{
+							*arg = std::get<std::string>(mDefaultValue);
+						}catch (std::bad_variant_access const& ex){
+							throw std::runtime_error("Bad Variant Access: Type not std::string");
+						}
+					}
+				},
+				[&](bool* arg){
+					try{
+						*arg = node->get_parameter(mParamDescriptor).as_bool();
+					}catch(rclcpp::exceptions::ParameterUninitializedException& e){
+						try{
+							*arg = std::get<bool>(mDefaultValue);
+						}catch (std::bad_variant_access const& ex){
+							throw std::runtime_error("Bad Variant Access: Type not bool");
+						}
+					}
+				},
+				[&](double* arg){
+					try{
+						*arg = node->get_parameter(mParamDescriptor).as_double();
+					}catch(rclcpp::exceptions::ParameterUninitializedException& e){
+						try{
+							*arg = std::get<double>(mDefaultValue);
+						}catch (std::bad_variant_access const& ex){
+							throw std::runtime_error("Bad Variant Access: Type not double");
+						}
+					}
+				},
+				[&](float* arg){
+					try{
+						*arg = static_cast<float>(node->get_parameter(mParamDescriptor).as_double());
+					}catch(rclcpp::exceptions::ParameterUninitializedException& e){
+						try{
+							*arg = std::get<float>(mDefaultValue);
+						}catch (std::bad_variant_access const& ex){
+							throw std::runtime_error("Bad Variant Access: Type not float");
+						}
+					}
+				}
+			}, mData);
+		}
+
+		static inline auto declareParameters(rclcpp::Node* node, std::vector<ParameterWrapper>& params) -> void{
+			RCLCPP_INFO(rclcpp::get_logger("param_logger"), "Declaring %zu parameters...", params.size());
+			for(auto& param : params){
+				node->declare_parameter(param.mParamDescriptor, param.mType);
+				param.visit(node);
+			}
+		}
+	};
+};
diff --git a/perception/object_detector/object_detector.cpp b/perception/object_detector/object_detector.cpp
@@ -0,0 +1,47 @@
+#include "object_detector.hpp"
+
+namespace mrover {
+
+    ObjectDetectorBase::ObjectDetectorBase() : rclcpp::Node(NODE_NAME), mLoopProfiler{get_logger()} {
+
+        std::vector<ParameterWrapper> params{
+                {"camera_frame", mCameraFrame, "zed_left_camera_frame"},
+                {"world_frame", mWorldFrame, "map"},
+                {"increment_weight", mObjIncrementWeight, 2},
+                {"decrement_weight", mObjDecrementWeight, 1},
+                {"hitcount_threshold", mObjHitThreshold, 5},
+                {"hitcount_max", mObjMaxHitcount, 10},
+                {"model_name", mModelName, "Large-Dataset"},
+                {"model_score_threshold", mModelScoreThreshold, 0.75},
+                {"model_nms_threshold", mModelNmsThreshold, 0.5}
+		};
+
+        ParameterWrapper::declareParameters(this, params);
+
+		std::filesystem::path packagePath = std::filesystem::path{ament_index_cpp::get_package_prefix("mrover")} / ".." / ".." / "src" / "mrover";
+
+        RCLCPP_INFO_STREAM(get_logger(), "Opening Model " << mModelName);
+
+        RCLCPP_INFO_STREAM(get_logger(), "Found package path " << packagePath);
+
+        mTensorRT = TensortRT{mModelName, packagePath.string()};
+
+        mDebugImgPub = create_publisher<sensor_msgs::msg::Image>("object_detector/debug_img", 1);
+
+        RCLCPP_INFO_STREAM(get_logger(), std::format("Object detector initialized with model: {} and thresholds: {} and {}", mModelName, mModelScoreThreshold, mModelNmsThreshold));
+    }
+
+    StereoObjectDetector::StereoObjectDetector() {
+        mSensorSub = create_subscription<sensor_msgs::msg::PointCloud2>("/camera/left/points", 1, [this](sensor_msgs::msg::PointCloud2::UniquePtr const& msg) {
+            StereoObjectDetector::pointCloudCallback(msg);
+        });
+    }
+} // namespace mrover
+
+
+auto main(int argc, char** argv) -> int {
+    rclcpp::init(argc, argv);
+    rclcpp::spin(std::make_shared<mrover::StereoObjectDetector>());
+    rclcpp::shutdown();
+    return EXIT_SUCCESS;
+}
diff --git a/perception/object_detector/object_detector.hpp b/perception/object_detector/object_detector.hpp
@@ -0,0 +1,67 @@
+#pragma once
+
+#include "pch.hpp"
+
+namespace mrover {
+
+    class ObjectDetectorBase : public rclcpp::Node {
+
+    protected:
+        static constexpr char const* NODE_NAME = "object_detector";
+
+        std::unique_ptr<tf2_ros::Buffer> mTfBuffer = std::make_unique<tf2_ros::Buffer>(get_clock());
+        std::shared_ptr<tf2_ros::TransformBroadcaster> mTfBroadcaster = std::make_shared<tf2_ros::TransformBroadcaster>(this);
+        std::shared_ptr<tf2_ros::TransformListener> mTfListener = std::make_shared<tf2_ros::TransformListener>(*mTfBuffer);
+
+        std::string mCameraFrame;
+        std::string mWorldFrame;
+
+        std::string mModelName;
+
+        LoopProfiler mLoopProfiler;
+
+        TensortRT mTensorRT;
+
+        cv::Mat mRgbImage, mImageBlob;
+        sensor_msgs::msg::Image mDetectionsImageMessage;
+
+        rclcpp::Publisher<sensor_msgs::msg::Image>::SharedPtr mDebugImgPub;
+        rclcpp::Subscription<sensor_msgs::msg::PointCloud2>::SharedPtr mSensorSub;
+
+        // TODO(quintin): Do not hard code exactly two classes
+        std::vector<int> mObjectHitCounts{0, 0};
+
+        int mObjIncrementWeight{};
+        int mObjDecrementWeight{};
+        int mObjHitThreshold{};
+        int mObjMaxHitcount{};
+        float mModelScoreThreshold{};
+        float mModelNmsThreshold{};
+
+        auto spiralSearchForValidPoint(sensor_msgs::msg::PointCloud2::UniquePtr const& cloudPtr,
+                                       std::size_t u, std::size_t v,
+                                       std::size_t width, std::size_t height) const -> std::optional<SE3d>;
+
+        auto updateHitsObject(sensor_msgs::msg::PointCloud2::UniquePtr const& msg,
+                              std::span<Detection const> detections,
+                              cv::Size const& imageSize = {640, 640}) -> void;
+
+        auto publishDetectedObjects(cv::InputArray image) -> void;
+
+        static auto drawDetectionBoxes(cv::InputOutputArray image, std::span<Detection const> detections) -> void;
+
+    public:
+        explicit ObjectDetectorBase();
+
+        ~ObjectDetectorBase() override = default;
+    };
+
+    class StereoObjectDetector final : public ObjectDetectorBase {
+    public:
+        explicit StereoObjectDetector();
+
+        static auto convertPointCloudToRGB(sensor_msgs::msg::PointCloud2::UniquePtr const& msg, cv::Mat const& image) -> void;
+
+        auto pointCloudCallback(sensor_msgs::msg::PointCloud2::UniquePtr const& msg) -> void;
+    };
+} // namespace mrover