From cfd335177cb233ee06a38a4f8267ea54e1b19f8e Mon Sep 17 00:00:00 2001
From: bkuangs <kuangbrian71@gmail.com>
Date: Tue, 20 Jan 2026 23:03:00 -0500
Subject: [PATCH] cleanup wk2

---
 obstacle_detector/CMakeLists.txt            |  3 ++-
 obstacle_detector/src/obstacle_detector.cpp | 15 ++++++++++--
 obstacle_detector/src/student_functions.cpp | 26 +++++++++++++++++++++
 obstacle_detector/src/student_functions.hpp | 14 +++++++++++
 4 files changed, 55 insertions(+), 3 deletions(-)
 create mode 100644 obstacle_detector/src/student_functions.cpp
 create mode 100644 obstacle_detector/src/student_functions.hpp

diff --git a/obstacle_detector/CMakeLists.txt b/obstacle_detector/CMakeLists.txt
index 4410ef272..8f0232c1b 100644
--- a/obstacle_detector/CMakeLists.txt
+++ b/obstacle_detector/CMakeLists.txt
@@ -14,6 +14,7 @@ find_package(OpenCV 4 REQUIRED)
 add_library(${PROJECT_NAME} SHARED
   src/obstacle_detector.cpp
 # BEGIN STUDENT CODE
+  src/student_functions.cpp
 # END STUDENT CODE
 )
 ament_target_dependencies(${PROJECT_NAME}
@@ -50,4 +51,4 @@ if(BUILD_TESTING)
   ament_lint_auto_find_test_dependencies()
 endif()
 
-ament_package()
+ament_package()
\ No newline at end of file
diff --git a/obstacle_detector/src/obstacle_detector.cpp b/obstacle_detector/src/obstacle_detector.cpp
index bf6ebac84..e2e963281 100644
--- a/obstacle_detector/src/obstacle_detector.cpp
+++ b/obstacle_detector/src/obstacle_detector.cpp
@@ -31,6 +31,7 @@
 #include <opencv2/core/eigen.hpp>
 
 // BEGIN STUDENT CODE
+#include "student_functions.hpp"
 // END STUDENT CODE
 
 namespace obstacle_detector
@@ -44,6 +45,13 @@ class ObstacleDetector : public rclcpp::Node
   {
     // BEGIN STUDENT CODE
     // Initialize publisher and subscriber
+    occupancy_grid_publisher_ = create_publisher<nav_msgs::msg::OccupancyGrid>("~/occupancy_grid", rclcpp::SystemDefaultsQoS());
+    camera_subscriber_ = image_transport::create_camera_subscription(
+        this, "/camera/image_raw",
+        std::bind(
+          &ObstacleDetector::ImageCallback, this, std::placeholders::_1,
+          std::placeholders::_2),
+        "raw", rclcpp::SensorDataQoS().get_rmw_qos_profile());
     // END STUDENT CODE
 
     declare_parameters<int>(
@@ -64,6 +72,8 @@ class ObstacleDetector : public rclcpp::Node
 
   // BEGIN STUDENT CODE
   // Declare subscriber and publisher members
+  rclcpp::Publisher<nav_msgs::msg::OccupancyGrid>::SharedPtr occupancy_grid_publisher_ {};
+  image_transport::CameraSubscriber camera_subscriber_ {};
   // END STUDENT CODE
 
   void ImageCallback(
@@ -83,7 +93,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Call FindColors()
-    cv::Mat detected_colors;
+    cv::Mat detected_colors = FindColors(cv_image->image, min_color, max_color);
     // END STUDENT CODE
 
     std::string tf_error_string;
@@ -117,7 +127,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Call ReprojectToGroundPlane
-    cv::Mat projected_colors;
+    cv::Mat projected_colors = ReprojectToGroundPlane(detected_colors, homography, map_size);
     // END STUDENT CODE
 
     cv::rotate(projected_colors, projected_colors, cv::ROTATE_90_CLOCKWISE);
@@ -141,6 +151,7 @@ class ObstacleDetector : public rclcpp::Node
 
     // BEGIN STUDENT CODE
     // Publish occupancy_grid_msg
+    occupancy_grid_publisher_->publish(occupancy_grid_msg);
     // END STUDENT CODE
   }
 
diff --git a/obstacle_detector/src/student_functions.cpp b/obstacle_detector/src/student_functions.cpp
new file mode 100644
index 000000000..84caf88b6
--- /dev/null
+++ b/obstacle_detector/src/student_functions.cpp
@@ -0,0 +1,26 @@
+#include "student_functions.hpp"
+
+cv::Mat FindColors(const cv::Mat input, const cv::Scalar range_min, const cv::Scalar range_max)
+{
+    cv::Mat input_hsv;
+    cv::cvtColor(input, input_hsv, cv::COLOR_BGR2HSV);
+
+    cv::Mat output(input.size(), CV_8UC1);
+
+    cv::inRange(input_hsv, range_min, range_max, output);
+
+    return output;
+}
+
+cv::Mat ReprojectToGroundPlane(const cv::Mat input,
+                             const cv::Mat homography,
+                             const cv::Size map_size)
+{
+    cv::Mat output(map_size, CV_8UC1);
+
+    cv::warpPerspective(
+        input, output, homography, map_size, cv::INTER_NEAREST, cv::BORDER_CONSTANT,
+        cv::Scalar(127));
+
+    return output;
+}
\ No newline at end of file
diff --git a/obstacle_detector/src/student_functions.hpp b/obstacle_detector/src/student_functions.hpp
new file mode 100644
index 000000000..fb311bfac
--- /dev/null
+++ b/obstacle_detector/src/student_functions.hpp
@@ -0,0 +1,14 @@
+#ifndef STUDENT_FUNCTIONS_HPP  // Check that the unique macro has not already been defined
+#define STUDENT_FUNCTIONS_HPP  // Define the unique macro
+
+#include <opencv2/opencv.hpp>
+
+cv::Mat FindColors(const cv::Mat input,
+                   const cv::Scalar range_min, 
+                   const cv::Scalar range_max);
+
+cv::Mat ReprojectToGroundPlane(const cv::Mat input,
+                             const cv::Mat homography,
+                             const cv::Size map_size);
+
+#endif  // Close the pre-processor if block
\ No newline at end of file